lhq/FT546-WCS
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(javaTest): clear blocked routes with real vehicle moves

Browse Source
This commit is contained in:
zgw
2026-07-31 14:25:33 +08:00
parent 1ede0c7fc9
commit cd2e6ce953
5 changed files with 386 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
javaTest/src/main/java/com/ft546/wcs/shadow/history/simulation/ClearanceMove.java Normal file
View File

@@ -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<String> route;
private int index;
private int protocolAttempts;
ClearanceMove(String vehicleId, String ownerTaskId, List<String> route) {
this.vehicleId = vehicleId;
this.ownerTaskId = ownerTaskId;
this.route = Collections.unmodifiableList(new ArrayList<String>(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<String> 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; }
}

67
javaTest/src/main/java/com/ft546/wcs/shadow/history/simulation/ClearanceMovePlanner.java Normal file
View File

@@ -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<ClearanceMove> plan(String vehicleId, String currentSite,
String ownerTaskId, List<String> ownerRemainingRoute,
Set<String> occupiedSites,
Set<ResourceKey> 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<String> excluded = new LinkedHashSet<String>(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<Candidate> candidates = new ArrayList<Candidate>();
List<String> siteIds = new ArrayList<String>(map.getSites().keySet());
Collections.sort(siteIds);
for (String target : siteIds) {
if (target.equals(currentSite) || excluded.contains(target)) continue;
List<String> 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<String> route;
private final boolean branch;
private Candidate(String target, List<String> route, boolean branch) {
this.target = target;
this.route = route;
this.branch = branch;
}
}
}

192
javaTest/src/main/java/com/ft546/wcs/shadow/history/simulation/HistoricalDispatchSimulator.java
View File

@@ -66,6 +66,7 @@ public final class HistoricalDispatchSimulator {
Map<String, SimulatedVehicleRuntime> vehicles = createRuntimes(input);
Map<String, ActiveTask> active = new LinkedHashMap<String, ActiveTask>();
Map<String, ClearanceMove> clearanceMoves = new LinkedHashMap<String, ClearanceMove>();
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<String> traversedEdges = new LinkedHashSet<String>();
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<String, SimulatedVehicleRuntime> vehicles,
Map<String, ActiveTask> active,
Map<String, ClearanceMove> clearanceMoves,
SimulationTaskQueue queue,
Map<String, Execution> 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<String, SimulatedVehicleRuntime> vehicles,
Map<String, ActiveTask> active,
Map<String, ClearanceMove> clearanceMoves,
Map<String, Execution> executions,
Map<String, SimulationTaskResult> terminal,
GridPathPlanner planner,
@@ -205,7 +219,8 @@ public final class HistoricalDispatchSimulator {
List<VehicleSnapshot> candidates = new ArrayList<VehicleSnapshot>();
Map<String, List<String>> approaches = new LinkedHashMap<String, List<String>>();
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<String> approach = planner.shortestPath(input.getGridMap(),
@@ -286,6 +301,8 @@ public final class HistoricalDispatchSimulator {
IdempotentWmsCompletionReceiver wms,
SimulationInvariantSummary invariants,
CorridorReservationManager reservations,
Map<String, ClearanceMove> clearanceMoves,
ClearanceMovePlanner clearancePlanner,
List<SimulationTickEvent> events,
Set<String> traversedEdges) {
boolean progress = false;
@@ -314,6 +331,18 @@ public final class HistoricalDispatchSimulator {
}
List<ResourceKey> resources = Arrays.asList(
ResourceKey.node(from), edge, ResourceKey.node(to));
Set<String> 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<String, SimulatedVehicleRuntime> vehicles,
Map<String, ClearanceMove> clearanceMoves,
ResourceLedger ledger,
SimulatedPlcGateway plc,
List<SimulationTickEvent> events,
Set<String> traversedEdges) {
List<String> vehicleIds = new ArrayList<String>(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<ResourceKey> 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<String, SimulatedVehicleRuntime> vehicles,
Map<String, ActiveTask> active,
Map<String, ClearanceMove> clearanceMoves,
ClearanceMovePlanner clearancePlanner,
ResourceLedger ledger,
HistoricalSimulationInput input,
Set<String> blockingVehicleIds,
List<SimulationTickEvent> events) {
List<String> blockers = new ArrayList<String>(blockingVehicleIds);
Collections.sort(blockers);
Set<String> occupiedSites = new LinkedHashSet<String>();
for (SimulatedVehicleRuntime vehicle : vehicles.values()) {
occupiedSites.add(vehicle.getCurrentSite());
}
Set<ResourceKey> reservedResources = new LinkedHashSet<ResourceKey>();
for (Map.Entry<ResourceKey, Set<String>> 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<ClearanceMove> 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<String> idleVehiclesAt(String siteId, String movingVehicleId,
Map<String, SimulatedVehicleRuntime> vehicles,
Map<String, ActiveTask> active,
Map<String, ClearanceMove> clearanceMoves) {
Set<String> blockers = new LinkedHashSet<String>();
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()