From cd2e6ce953c77b7f9505ba6df4ad6d0d4aa645ac Mon Sep 17 00:00:00 2001 From: zgw <13623675548@163.com> Date: Fri, 31 Jul 2026 14:25:33 +0800 Subject: [PATCH] feat(javaTest): clear blocked routes with real vehicle moves --- .../history/simulation/ClearanceMove.java | 34 ++++ .../simulation/ClearanceMovePlanner.java | 67 ++++++ .../HistoricalDispatchSimulator.java | 192 +++++++++++++++++- .../simulation/ClearanceMovePlannerTest.java | 37 ++++ .../HistoricalDispatchSimulatorTest.java | 62 ++++++ 5 files changed, 386 insertions(+), 6 deletions(-) create mode 100644 javaTest/src/main/java/com/ft546/wcs/shadow/history/simulation/ClearanceMove.java create mode 100644 javaTest/src/main/java/com/ft546/wcs/shadow/history/simulation/ClearanceMovePlanner.java create mode 100644 javaTest/src/test/java/com/ft546/wcs/shadow/history/simulation/ClearanceMovePlannerTest.java diff --git a/javaTest/src/main/java/com/ft546/wcs/shadow/history/simulation/ClearanceMove.java b/javaTest/src/main/java/com/ft546/wcs/shadow/history/simulation/ClearanceMove.java new file mode 100644 index 0000000..b90a285 --- /dev/null +++ b/javaTest/src/main/java/com/ft546/wcs/shadow/history/simulation/ClearanceMove.java @@ -0,0 +1,34 @@ +package com.ft546.wcs.shadow.history.simulation; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; + +public final class ClearanceMove { + private final String vehicleId; + private final String ownerTaskId; + private final List route; + private int index; + private int protocolAttempts; + + ClearanceMove(String vehicleId, String ownerTaskId, List route) { + this.vehicleId = vehicleId; + this.ownerTaskId = ownerTaskId; + this.route = Collections.unmodifiableList(new ArrayList(route)); + } + + public String getVehicleId() { return vehicleId; } + public String getOwnerTaskId() { return ownerTaskId; } + public String getSourceSite() { return route.get(0); } + public String getTargetSite() { return route.get(route.size() - 1); } + public List getRoute() { return route; } + public int getIndex() { return index; } + public int getProtocolAttempts() { return protocolAttempts; } + public boolean isComplete() { return index >= route.size() - 1; } + + String currentSite() { return route.get(index); } + String nextSite() { return route.get(index + 1); } + void moved() { index++; } + void attemptedProtocol() { protocolAttempts++; } + String clearanceTaskId() { return "CLEARANCE:" + ownerTaskId + ":" + vehicleId; } +} diff --git a/javaTest/src/main/java/com/ft546/wcs/shadow/history/simulation/ClearanceMovePlanner.java b/javaTest/src/main/java/com/ft546/wcs/shadow/history/simulation/ClearanceMovePlanner.java new file mode 100644 index 0000000..c116e59 --- /dev/null +++ b/javaTest/src/main/java/com/ft546/wcs/shadow/history/simulation/ClearanceMovePlanner.java @@ -0,0 +1,67 @@ +package com.ft546.wcs.shadow.history.simulation; + +import com.ft546.wcs.demo.traffic.GridMap; +import com.ft546.wcs.demo.traffic.GridPathPlanner; +import com.ft546.wcs.shadow.resource.ResourceKey; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.Comparator; +import java.util.LinkedHashSet; +import java.util.List; +import java.util.Optional; +import java.util.Set; + +public final class ClearanceMovePlanner { + public Optional plan(String vehicleId, String currentSite, + String ownerTaskId, List ownerRemainingRoute, + Set occupiedSites, + Set reservedResources, + GridMap map) { + if (vehicleId == null || currentSite == null || ownerTaskId == null + || ownerRemainingRoute == null || occupiedSites == null + || reservedResources == null || map == null) { + throw new IllegalArgumentException("clearance planning inputs are required"); + } + Set excluded = new LinkedHashSet(ownerRemainingRoute); + excluded.addAll(occupiedSites); + for (ResourceKey resource : reservedResources) { + if (resource.value().startsWith("NODE:")) { + excluded.add(resource.value().substring("NODE:".length())); + } + } + excluded.remove(currentSite); + + GridPathPlanner planner = new GridPathPlanner(); + List candidates = new ArrayList(); + List siteIds = new ArrayList(map.getSites().keySet()); + Collections.sort(siteIds); + for (String target : siteIds) { + if (target.equals(currentSite) || excluded.contains(target)) continue; + List route = planner.shortestPath(map, currentSite, target, excluded); + if (!route.isEmpty()) { + candidates.add(new Candidate(target, route, + map.getNeighbors(target).size() <= 1)); + } + } + Collections.sort(candidates, Comparator + .comparingInt((Candidate candidate) -> candidate.route.size()) + .thenComparing(candidate -> candidate.branch ? 0 : 1) + .thenComparing(candidate -> candidate.target)); + if (candidates.isEmpty()) return Optional.empty(); + return Optional.of(new ClearanceMove(vehicleId, ownerTaskId, + candidates.get(0).route)); + } + + private static final class Candidate { + private final String target; + private final List route; + private final boolean branch; + + private Candidate(String target, List route, boolean branch) { + this.target = target; + this.route = route; + this.branch = branch; + } + } +} diff --git a/javaTest/src/main/java/com/ft546/wcs/shadow/history/simulation/HistoricalDispatchSimulator.java b/javaTest/src/main/java/com/ft546/wcs/shadow/history/simulation/HistoricalDispatchSimulator.java index 9d6c47d..5f1527b 100644 --- a/javaTest/src/main/java/com/ft546/wcs/shadow/history/simulation/HistoricalDispatchSimulator.java +++ b/javaTest/src/main/java/com/ft546/wcs/shadow/history/simulation/HistoricalDispatchSimulator.java @@ -66,6 +66,7 @@ public final class HistoricalDispatchSimulator { Map vehicles = createRuntimes(input); Map active = new LinkedHashMap(); + Map clearanceMoves = new LinkedHashMap(); ResourceLedger ledger = new ResourceLedger(input.getTrafficMap().getCapacities()); SimulationInvariantSummary invariants = new SimulationInvariantSummary(); occupyInitialNodes(vehicles, ledger, invariants); @@ -75,6 +76,7 @@ public final class HistoricalDispatchSimulator { GridPathPlanner planner = new GridPathPlanner(); VehicleAssignmentService assignment = new VehicleAssignmentService(); CorridorReservationManager reservations = new CorridorReservationManager(); + ClearanceMovePlanner clearancePlanner = new ClearanceMovePlanner(); long now = 0L; long tick = 0L; @@ -83,13 +85,17 @@ public final class HistoricalDispatchSimulator { config.getNoProgressLimit(), terminal.size()); while (terminal.size() < expected && tick < config.getTickLimit()) { applyVehicleEvents(tick, now, faults, vehicles, active, - queue, executions, reservations, events); + clearanceMoves, queue, executions, reservations, events); assignReadyTasks(tick, now, input, queue, vehicles, active, - executions, terminal, planner, assignment, plc, reservations, events); + clearanceMoves, executions, terminal, planner, assignment, plc, + reservations, events); Set traversedEdges = new LinkedHashSet(); + advanceClearanceMoves(tick, now, input, config, vehicles, clearanceMoves, + ledger, plc, events, traversedEdges); advanceActiveTasks(tick, now, input, config, queue, vehicles, active, executions, terminal, planner, ledger, plc, archive, wms, - invariants, reservations, events, traversedEdges); + invariants, reservations, clearanceMoves, clearancePlanner, + events, traversedEdges); verifyLedger(ledger, invariants); if (watchdog.observe(terminal.size())) { @@ -138,6 +144,7 @@ public final class HistoricalDispatchSimulator { private boolean applyVehicleEvents(long tick, long now, DeterministicFaultPlan faults, Map vehicles, Map active, + Map clearanceMoves, SimulationTaskQueue queue, Map executions, CorridorReservationManager reservations, @@ -161,6 +168,12 @@ public final class HistoricalDispatchSimulator { event.getType().name(), vehicle.getCurrentSite(), vehicle.getCurrentSite())); changed = true; if (!vehicle.isAssignmentEligible()) { + ClearanceMove clearance = clearanceMoves.remove(vehicle.getVehicleId()); + if (clearance != null) { + events.add(event(tick, now, "CLEARANCE_HOLD", clearance.getOwnerTaskId(), + vehicle.getVehicleId(), event.getType().name(), + vehicle.getCurrentSite(), null)); + } ActiveTask stopped = active.remove(vehicle.getVehicleId()); if (stopped != null) { reservations.releaseTask(stopped.execution.task.getSimulationTaskId()); @@ -179,6 +192,7 @@ public final class HistoricalDispatchSimulator { SimulationTaskQueue queue, Map vehicles, Map active, + Map clearanceMoves, Map executions, Map terminal, GridPathPlanner planner, @@ -205,7 +219,8 @@ public final class HistoricalDispatchSimulator { List candidates = new ArrayList(); Map> approaches = new LinkedHashMap>(); for (SimulatedVehicleRuntime vehicle : vehicles.values()) { - if (!vehicle.isAssignmentEligible() || active.containsKey(vehicle.getVehicleId())) { + if (!vehicle.isAssignmentEligible() || active.containsKey(vehicle.getVehicleId()) + || clearanceMoves.containsKey(vehicle.getVehicleId())) { continue; } List approach = planner.shortestPath(input.getGridMap(), @@ -286,6 +301,8 @@ public final class HistoricalDispatchSimulator { IdempotentWmsCompletionReceiver wms, SimulationInvariantSummary invariants, CorridorReservationManager reservations, + Map clearanceMoves, + ClearanceMovePlanner clearancePlanner, List events, Set traversedEdges) { boolean progress = false; @@ -314,6 +331,18 @@ public final class HistoricalDispatchSimulator { } List resources = Arrays.asList( ResourceKey.node(from), edge, ResourceKey.node(to)); + Set idleBlockers = idleVehiclesAt(to, vehicleId, vehicles, + active, clearanceMoves); + if (!idleBlockers.isEmpty()) { + boolean clearancePlanned = planClearanceForIdleBlockers( + tick, now, current, vehicles, active, clearanceMoves, + clearancePlanner, ledger, input, idleBlockers, events); + recordHold(tick, now, current, vehicle, active, queue, + clearancePlanned ? "IDLE_VEHICLE_CLEARANCE" + : "NO_CLEARANCE_SITE", + reservations, events); + continue; + } Instant decisionTime = Instant.EPOCH.plusMillis(now + tick); TrafficVehicleSnapshot frozen = new TrafficVehicleSnapshot(vehicleId, Long.toString(vehicle.getBootId()), tick + 1, tick + 1, @@ -340,9 +369,14 @@ public final class HistoricalDispatchSimulator { } ResourceLedger.ProtectionResult protection = ledger.evaluate(vehicleId, resources); if (!protection.isGranted()) { - invariants.capacityViolation(); + boolean clearancePlanned = planClearanceForIdleBlockers( + tick, now, current, vehicles, active, clearanceMoves, + clearancePlanner, ledger, input, + protection.getBlockingVehicles(), events); recordHold(tick, now, current, vehicle, active, queue, - "AUTHORITATIVE_RESOURCE_REJECTED", reservations, events); + clearancePlanned ? "IDLE_VEHICLE_CLEARANCE" + : "AUTHORITATIVE_RESOURCE_REJECTED", + reservations, events); continue; } SimulatedPlcGateway.PlcResponse sent = plc.send( @@ -387,6 +421,152 @@ public final class HistoricalDispatchSimulator { return progress; } + private void advanceClearanceMoves(long tick, long now, + HistoricalSimulationInput input, + SimulationConfig config, + Map vehicles, + Map clearanceMoves, + ResourceLedger ledger, + SimulatedPlcGateway plc, + List events, + Set traversedEdges) { + List vehicleIds = new ArrayList(clearanceMoves.keySet()); + Collections.sort(vehicleIds); + for (String vehicleId : vehicleIds) { + ClearanceMove clearance = clearanceMoves.get(vehicleId); + SimulatedVehicleRuntime vehicle = vehicles.get(vehicleId); + if (clearance == null || vehicle == null || !vehicle.isAssignmentEligible()) continue; + if (clearance.isComplete()) { + clearanceMoves.remove(vehicleId); + continue; + } + String from = clearance.currentSite(); + String to = clearance.nextSite(); + String direction = from + "->" + to; + if (traversedEdges.contains(to + "->" + from) + || traversedEdges.contains(direction)) { + events.add(event(tick, now, "CLEARANCE_HOLD", clearance.getOwnerTaskId(), + vehicleId, "OPPOSITE_EDGE_WINDOW", from, to)); + continue; + } + ResourceKey edge = ResourceKey.edge(from, to); + List resources = Arrays.asList( + ResourceKey.node(from), edge, ResourceKey.node(to)); + Instant decisionTime = Instant.EPOCH.plusMillis(now + tick); + TrafficVehicleSnapshot frozen = new TrafficVehicleSnapshot(vehicleId, + Long.toString(vehicle.getBootId()), tick + 1, tick + 1, + decisionTime, from, to, + clearance.getRoute().subList(clearance.getIndex(), clearance.getRoute().size()), + true, false, false); + TrafficSnapshot snapshot = new TrafficSnapshot(tick + 1, decisionTime, + input.getTrafficMap().getMapCode(), input.getTrafficMap().getVersionId(), + "history-clearance-v1", Collections.singletonList(frozen)); + ShadowCandidate candidate = ShadowCandidate.waiting(clearance.clearanceTaskId(), + vehicleId, false, Integer.MAX_VALUE, now, + clearance.getRoute().size() - clearance.getIndex(), resources, null); + ShadowTrafficCoordinator coordinator = new ShadowTrafficCoordinator( + input.getTrafficMap(), ledger, new RollingDispatchPolicy()); + ShadowDecision decision = coordinator.run(snapshot, + Collections.singletonList(candidate)).getDecisions().get(0); + if (decision.getType() != DecisionType.GRANT) { + events.add(event(tick, now, "CLEARANCE_HOLD", clearance.getOwnerTaskId(), + vehicleId, decision.getReasonCode(), from, to)); + continue; + } + ResourceLedger.ProtectionResult protection = ledger.evaluate(vehicleId, resources); + if (!protection.isGranted()) { + events.add(event(tick, now, "CLEARANCE_HOLD", clearance.getOwnerTaskId(), + vehicleId, "AUTHORITATIVE_RESOURCE_REJECTED", from, to)); + continue; + } + clearance.attemptedProtocol(); + SimulatedPlcGateway.PlcResponse sent = plc.send( + clearance.clearanceTaskId(), vehicle, from, to); + SimulatedPlcGateway.PlcResponse accepted = plc.accept(sent, vehicle); + if (accepted.getReason() != SimulatedPlcGateway.PlcReason.ACK) { + ledger.release(vehicleId, Arrays.asList(edge, ResourceKey.node(to))); + events.add(event(tick, now, "CLEARANCE_PLC_RETRY", + clearance.getOwnerTaskId(), vehicleId, + accepted.getReason().name(), from, to)); + continue; + } + Site nextSite = input.getGridMap().getSite(to); + vehicle.moveTo(to, nextSite.getFloor()); + ledger.release(vehicleId, Arrays.asList(ResourceKey.node(from), edge)); + traversedEdges.add(direction); + clearance.moved(); + events.add(event(tick, now, "CLEARANCE_MOVE", clearance.getOwnerTaskId(), + vehicleId, "ACK", from, to)); + if (clearance.isComplete()) { + clearanceMoves.remove(vehicleId); + events.add(event(tick, now, "CLEARANCE_COMPLETE", + clearance.getOwnerTaskId(), vehicleId, "ROUTE_CLEARED", + to, to)); + } + } + } + + private boolean planClearanceForIdleBlockers( + long tick, long now, ActiveTask owner, + Map vehicles, + Map active, + Map clearanceMoves, + ClearanceMovePlanner clearancePlanner, + ResourceLedger ledger, + HistoricalSimulationInput input, + Set blockingVehicleIds, + List events) { + List blockers = new ArrayList(blockingVehicleIds); + Collections.sort(blockers); + Set occupiedSites = new LinkedHashSet(); + for (SimulatedVehicleRuntime vehicle : vehicles.values()) { + occupiedSites.add(vehicle.getCurrentSite()); + } + Set reservedResources = new LinkedHashSet(); + for (Map.Entry> entry : ledger.snapshotHolders().entrySet()) { + if (!entry.getValue().isEmpty()) reservedResources.add(entry.getKey()); + } + for (String blockerId : blockers) { + SimulatedVehicleRuntime blocker = vehicles.get(blockerId); + if (blocker == null || !blocker.isAssignmentEligible() + || active.containsKey(blockerId) || clearanceMoves.containsKey(blockerId)) { + continue; + } + java.util.Optional planned = clearancePlanner.plan( + blockerId, blocker.getCurrentSite(), + owner.execution.task.getSimulationTaskId(), + owner.route.subList(owner.index, owner.route.size()), + occupiedSites, reservedResources, input.getGridMap()); + if (planned.isPresent()) { + ClearanceMove clearance = planned.get(); + clearanceMoves.put(blockerId, clearance); + events.add(event(tick, now, "CLEARANCE_PLANNED", + owner.execution.task.getSimulationTaskId(), blockerId, + "IDLE_BLOCKER", clearance.getSourceSite(), clearance.getTargetSite())); + return true; + } + } + return false; + } + + private Set idleVehiclesAt(String siteId, String movingVehicleId, + Map vehicles, + Map active, + Map clearanceMoves) { + Set blockers = new LinkedHashSet(); + for (SimulatedVehicleRuntime candidate : vehicles.values()) { + String candidateId = candidate.getVehicleId(); + if (!candidateId.equals(movingVehicleId) + && siteId.equals(candidate.getCurrentSite()) + && candidate.isAssignmentEligible() + && !active.containsKey(candidateId) + && !clearanceMoves.containsKey(candidateId)) { + blockers.add(candidateId); + } + } + return blockers; + } + private int queuePriority(Execution execution, long now, SimulationConfig config) { long waited = Math.max(0L, now - execution.arrivalMillis); return (int) Math.max(0L, (long) execution.task.getPriority() diff --git a/javaTest/src/test/java/com/ft546/wcs/shadow/history/simulation/ClearanceMovePlannerTest.java b/javaTest/src/test/java/com/ft546/wcs/shadow/history/simulation/ClearanceMovePlannerTest.java new file mode 100644 index 0000000..7a8e026 --- /dev/null +++ b/javaTest/src/test/java/com/ft546/wcs/shadow/history/simulation/ClearanceMovePlannerTest.java @@ -0,0 +1,37 @@ +package com.ft546.wcs.shadow.history.simulation; + +import com.ft546.wcs.demo.model.Site; +import com.ft546.wcs.demo.traffic.GridMap; +import com.ft546.wcs.shadow.resource.ResourceKey; +import org.junit.jupiter.api.Test; + +import java.util.Arrays; +import java.util.Collections; +import java.util.LinkedHashSet; +import java.util.Optional; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +class ClearanceMovePlannerTest { + @Test + void selectsAReachableOffRouteUnoccupiedSite() { + GridMap map = new GridMap(); + for (String site : Arrays.asList("A", "B", "C", "X")) { + map.addSite(new Site(site, 1, site.charAt(0), 1, "TEST")); + } + map.connect("A", "B"); + map.connect("B", "C"); + map.connect("B", "X"); + + Optional planned = new ClearanceMovePlanner().plan( + "3", "B", "OWNER", Arrays.asList("A", "B", "C"), + new LinkedHashSet(Arrays.asList("A", "B")), + Collections.singleton(ResourceKey.node("C")), map); + + assertTrue(planned.isPresent()); + assertEquals("X", planned.get().getTargetSite()); + assertEquals(Arrays.asList("B", "X"), planned.get().getRoute()); + assertEquals("OWNER", planned.get().getOwnerTaskId()); + } +} diff --git a/javaTest/src/test/java/com/ft546/wcs/shadow/history/simulation/HistoricalDispatchSimulatorTest.java b/javaTest/src/test/java/com/ft546/wcs/shadow/history/simulation/HistoricalDispatchSimulatorTest.java index d57c680..332e351 100644 --- a/javaTest/src/test/java/com/ft546/wcs/shadow/history/simulation/HistoricalDispatchSimulatorTest.java +++ b/javaTest/src/test/java/com/ft546/wcs/shadow/history/simulation/HistoricalDispatchSimulatorTest.java @@ -68,6 +68,35 @@ class HistoricalDispatchSimulatorTest { assertEquals(1, result.getReplannedCount()); } + @Test + void movesIdleBlockersNodeByNodeAndCompletesBothBusinessDirections() { + HistoricalSimulationInput input = blockedThreeVehicleCorridorInput(); + + HistoricalDispatchSimulationResult result = new HistoricalDispatchSimulator().simulate( + input, new SimulationConfig(100, 30_000L, 3, 200, 500), + DeterministicFaultPlan.none()); + + assertEquals(2, result.getCompletedCount(), () -> result.canonicalView() + "\n" + + result.getEvents().stream() + .map(event -> event.getTick() + ":" + event.getType() + ":" + + event.getTaskId() + ":" + event.getVehicleId() + ":" + + event.getReason() + ":" + event.getFromSite() + "->" + + event.getToSite()) + .collect(java.util.stream.Collectors.joining("\n"))); + assertEquals(0, result.getWaitingCount()); + assertTrue(result.getEvents().stream() + .anyMatch(event -> "CLEARANCE_PLANNED".equals(event.getType()))); + assertTrue(result.getEvents().stream() + .anyMatch(event -> "CLEARANCE_MOVE".equals(event.getType()))); + result.getEvents().stream() + .filter(event -> "CLEARANCE_MOVE".equals(event.getType())) + .forEach(event -> assertTrue(input.getGridMap().getNeighbors(event.getFromSite()) + .contains(event.getToSite()))); + assertEquals(0, result.getInvariants().getOppositeEdgeViolations()); + assertEquals(0, result.getInvariants().getNodeDoubleOccupancyViolations()); + assertEquals(0, result.getInvariants().getCapacityViolations()); + } + static HistoricalSimulationInput controlledInput() { GridMap grid = new GridMap(); for (String node : Arrays.asList("A", "B", "C", "D")) { @@ -151,6 +180,39 @@ class HistoricalDispatchSimulatorTest { "1", 1, "A", 5, 1, 100, true, true, false))); } + private HistoricalSimulationInput blockedThreeVehicleCorridorInput() { + GridMap grid = new GridMap(); + for (String node : Arrays.asList("A", "B", "C", "D", "X", "Z")) { + grid.addSite(new Site(node, 1, node.charAt(0), 1, "TEST")); + } + grid.connect("A", "B"); + grid.connect("B", "C"); + grid.connect("C", "D"); + grid.connect("B", "X"); + grid.connect("D", "Z"); + LinkedHashSet nodes = new LinkedHashSet( + Arrays.asList("A", "B", "C", "D", "X", "Z")); + LinkedHashSet edges = + new LinkedHashSet(); + edges.add(new TrafficMapCompiler.EdgeDefinition("A", "B")); + edges.add(new TrafficMapCompiler.EdgeDefinition("B", "C")); + edges.add(new TrafficMapCompiler.EdgeDefinition("C", "D")); + edges.add(new TrafficMapCompiler.EdgeDefinition("B", "X")); + edges.add(new TrafficMapCompiler.EdgeDefinition("D", "Z")); + EffectiveTrafficMap traffic = new TrafficMapCompiler().compile( + new TrafficMapCompiler.MapDefinition("BLOCKED", 1L, "TEST", true, + "test", "test", nodes, edges, Collections.emptyMap(), + Collections.emptySet(), Collections.emptySet())); + List tasks = Arrays.asList( + customTask("OWNER", "A", "D", 1, 0, Arrays.asList("A", "B", "C", "D")), + customTask("RETURN", "D", "A", 2, 0, Arrays.asList("D", "C", "B", "A"))); + List vehicles = Arrays.asList( + new VehicleSnapshot("1", 1, "A", 5, 1, 100, true, true, false), + new VehicleSnapshot("2", 1, "D", 5, 1, 100, true, true, false), + new VehicleSnapshot("3", 1, "B", 5, 1, 100, true, true, false)); + return new HistoricalSimulationInput(tasks, Collections.emptyList(), grid, traffic, vehicles); + } + private static HistoricalSimulationTask task(String id, String source, String target, int priority, int sequence) { List path = "T2".equals(id)