From 15bf47fa25f769d1546b77ea694407393da86cc9 Mon Sep 17 00:00:00 2001 From: NoyGabay <104091867+NoyGabay@users.noreply.github.com> Date: Tue, 22 Oct 2024 10:42:49 +0300 Subject: [PATCH] lacam constraints support (#39) * add constraints support in lacam, do constraints checks only if necessary for efficiency * Update src/main/java/BasicMAPF/Solvers/LaCAM/LaCAM_Solver.java Co-authored-by: J-morag <38430800+J-morag@users.noreply.github.com> * Update src/main/java/BasicMAPF/Solvers/LaCAM/LaCAM_Solver.java Co-authored-by: J-morag <38430800+J-morag@users.noreply.github.com> * Update src/main/java/BasicMAPF/Solvers/LaCAM/LaCAM_Solver.java Co-authored-by: J-morag <38430800+J-morag@users.noreply.github.com> * Update src/main/java/BasicMAPF/Solvers/LaCAM/LaCAM_Solver.java Co-authored-by: J-morag <38430800+J-morag@users.noreply.github.com> * refactoring and removing not necessary constraints set * add constraints handling test --------- Co-authored-by: J-morag <38430800+J-morag@users.noreply.github.com> --- .../Solvers/LaCAM/LaCAMStar_Solver.java | 11 +- .../BasicMAPF/Solvers/LaCAM/LaCAM_Solver.java | 148 +++++++++++++----- .../Solvers/LaCAM/LaCAM_SolverTest.java | 18 ++- .../java/BasicMAPF/TestConstants/Agents.java | 1 + 4 files changed, 134 insertions(+), 44 deletions(-) diff --git a/src/main/java/BasicMAPF/Solvers/LaCAM/LaCAMStar_Solver.java b/src/main/java/BasicMAPF/Solvers/LaCAM/LaCAMStar_Solver.java index 08a3873c..59206b18 100644 --- a/src/main/java/BasicMAPF/Solvers/LaCAM/LaCAMStar_Solver.java +++ b/src/main/java/BasicMAPF/Solvers/LaCAM/LaCAMStar_Solver.java @@ -52,7 +52,6 @@ protected Solution runAlgorithm(MAPF_Instance instance, RunParameters parameters HashMap initialConfiguration = new HashMap<>(); for (Agent agent : instance.agents) { initialConfiguration.put(agent, instance.map.getMapLocation(agent.source)); - this.lacamSolver.goalConfiguration.put(agent, instance.map.getMapLocation(agent.target)); this.lacamSolver.agents.put(agent.iD, agent); } LowLevelNode C_init = this.lacamSolver.initNewLowLevelNode(); @@ -238,14 +237,14 @@ private float calcHValue(HashMap currentConfiguration, HighLe float cost = 0; if (this.lacamSolver.transientMAPFSettings.isTransientMAPF() && parent != null) { for (Agent agent : currentConfiguration.keySet()) { - if (currentConfiguration.get(agent) != this.lacamSolver.goalConfiguration.get(agent) && !parent.reachedGoalsMap.get(agent)) { - cost += this.lacamSolver.heuristic.getHToTargetFromLocation(this.lacamSolver.goalConfiguration.get(agent).getCoordinate(), currentConfiguration.get(agent)); + if (currentConfiguration.get(agent) != this.lacamSolver.getAgentsTarget(agent) && !parent.reachedGoalsMap.get(agent)) { + cost += this.lacamSolver.heuristic.getHToTargetFromLocation(this.lacamSolver.getAgentsTarget(agent).getCoordinate(), currentConfiguration.get(agent)); } } } else { for (Agent agent : currentConfiguration.keySet()) { - cost += this.lacamSolver.heuristic.getHToTargetFromLocation(this.lacamSolver.goalConfiguration.get(agent).getCoordinate(), currentConfiguration.get(agent)); + cost += this.lacamSolver.heuristic.getHToTargetFromLocation(this.lacamSolver.getAgentsTarget(agent).getCoordinate(), currentConfiguration.get(agent)); } } return cost; @@ -264,7 +263,7 @@ private int getEdgeCost(HighLevelNodeStar HNode_from, HashMap if (this.lacamSolver.transientMAPFSettings.isTransientMAPF()) { for (Agent agent : configuration_from.keySet()) { // if the next location of an agent is NOT its goal and the agent did not visit its goal - if (configuration_to.get(agent) != this.lacamSolver.goalConfiguration.get(agent) && !HNode_from.reachedGoalsMap.get(agent)) { + if (configuration_to.get(agent) != this.lacamSolver.getAgentsTarget(agent) && !HNode_from.reachedGoalsMap.get(agent)) { cost++; } } @@ -272,7 +271,7 @@ private int getEdgeCost(HighLevelNodeStar HNode_from, HashMap else { for (Agent agent : configuration_from.keySet()) { // if the location of an agent in both current and next configuration is NOT goal location - if (configuration_from.get(agent) != this.lacamSolver.goalConfiguration.get(agent) || configuration_to.get(agent) != this.lacamSolver.goalConfiguration.get(agent)) { + if (configuration_from.get(agent) != this.lacamSolver.getAgentsTarget(agent) || configuration_to.get(agent) != this.lacamSolver.getAgentsTarget(agent)) { cost++; } } diff --git a/src/main/java/BasicMAPF/Solvers/LaCAM/LaCAM_Solver.java b/src/main/java/BasicMAPF/Solvers/LaCAM/LaCAM_Solver.java index c6093c48..5c888d8e 100644 --- a/src/main/java/BasicMAPF/Solvers/LaCAM/LaCAM_Solver.java +++ b/src/main/java/BasicMAPF/Solvers/LaCAM/LaCAM_Solver.java @@ -7,13 +7,14 @@ import BasicMAPF.Instances.MAPF_Instance; import BasicMAPF.Instances.Maps.I_Location; import BasicMAPF.Solvers.AStar.CostsAndHeuristics.DistanceTableSingleAgentHeuristic; +import BasicMAPF.Solvers.AStar.CostsAndHeuristics.SingleAgentGAndH; import BasicMAPF.Solvers.A_Solver; -import BasicMAPF.Solvers.ConstraintsAndConflicts.Constraint.Constraint; import BasicMAPF.Solvers.ConstraintsAndConflicts.Constraint.ConstraintSet; import BasicMAPF.Solvers.ConstraintsAndConflicts.Constraint.I_ConstraintSet; import Environment.Metrics.InstanceReport; import TransientMAPF.TransientMAPFSettings; import TransientMAPF.TransientMAPFSolution; +import org.jetbrains.annotations.NotNull; import org.jetbrains.annotations.Nullable; import java.util.*; @@ -35,14 +36,18 @@ public class LaCAM_Solver extends A_Solver { protected HashMap, HighLevelNode> explored; /** - * heuristic to use in the low level search to find the closest nodes to an agent's goal + * HashMap to manage configurations that the algorithm already saw. + * This map is used when the solver need to handle constraints, so time step is included in the configuration. + * For each configuration, we can save several High level nodes, each one with different time step. */ - protected DistanceTableSingleAgentHeuristic heuristic; + protected HashMap, List> exploredWithExternalConstraints; + + protected boolean needToHandleConstraints; /** - * Map saving for each agent its goal location, representing the goal configuration. + * heuristic to use in the low level search to find the closest nodes to an agent's goal */ - protected HashMap goalConfiguration; + protected SingleAgentGAndH heuristic; /** * Map saving for each ID its agent as object. @@ -58,20 +63,14 @@ public class LaCAM_Solver extends A_Solver { * variable indicates whether the solution returned by the algorithm is transient. */ protected final TransientMAPFSettings transientMAPFSettings; - - protected I_ConstraintSet solverConstraints; + protected I_ConstraintSet constraintsSet; protected MAPF_Instance instance; protected int failedToFindConfigCounter; protected long totalTimeFindConfigurations; - protected HashMap occupiedNowConfig; protected HashMap occupiedNextConfig; - - protected ConstraintSet instanceConstraints; - protected int improveVisitsCounter; - protected int timeStep; /** * Constructor. @@ -86,34 +85,30 @@ public class LaCAM_Solver extends A_Solver { protected void init(MAPF_Instance instance, RunParameters parameters){ super.init(instance, parameters); - this.solverConstraints = parameters.constraints == null ? new ConstraintSet(): parameters.constraints; + this.constraintsSet = parameters.constraints == null ? new ConstraintSet(): parameters.constraints; this.open = new Stack<>(); + this.needToHandleConstraints = !this.constraintsSet.isEmpty(); this.explored = new HashMap<>(); - this.goalConfiguration = new HashMap<>(); + if (!this.constraintsSet.isEmpty()) { + this.exploredWithExternalConstraints = new HashMap<>(); + } this.agents = new HashMap<>(); this.failedToFindConfigCounter = 0; this.totalTimeFindConfigurations = 0; this.instance = instance; this.occupiedNowConfig = new HashMap<>(); this.occupiedNextConfig = new HashMap<>(); - this.instanceConstraints = null; this.timeStep = parameters.problemStartTime + 1; this.improveVisitsCounter = 0; // distance between every vertex in the graph to each agent's goal - if (parameters.singleAgentGAndH instanceof DistanceTableSingleAgentHeuristic) { - this.heuristic = (DistanceTableSingleAgentHeuristic) parameters.singleAgentGAndH; - } - else { - this.heuristic = new DistanceTableSingleAgentHeuristic(instance.agents, instance.map); - } + this.heuristic = Objects.requireNonNullElseGet(parameters.singleAgentGAndH, () -> new DistanceTableSingleAgentHeuristic(instance.agents, instance.map)); } @Override protected Solution runAlgorithm(MAPF_Instance instance, RunParameters parameters) { HashMap initialConfiguration = new HashMap<>(); for (Agent agent : instance.agents) { initialConfiguration.put(agent, instance.map.getMapLocation(agent.source)); - this.goalConfiguration.put(agent, instance.map.getMapLocation(agent.target)); this.agents.put(agent.iD, agent); } LowLevelNode C_init = initNewLowLevelNode(); @@ -121,7 +116,19 @@ protected Solution runAlgorithm(MAPF_Instance instance, RunParameters parameters ArrayList order = sortByPriority(priorities); HighLevelNode N_init = initNewHighLevelNode(initialConfiguration, C_init,order, priorities, null); this.open.push(N_init); - this.explored.put(initialConfiguration, N_init); + + if (this.needToHandleConstraints) { + List nodesList = this.exploredWithExternalConstraints.get(initialConfiguration); + if (nodesList == null) { + nodesList = new ArrayList<>(); + this.exploredWithExternalConstraints.put(initialConfiguration, nodesList); + } + nodesList.add(N_init); + this.explored.put(initialConfiguration, N_init); + } + else { + this.explored.put(initialConfiguration, N_init); + } while (!this.open.empty() && !checkTimeout()) { HighLevelNode N = this.open.peek(); @@ -187,7 +194,21 @@ protected Solution runAlgorithm(MAPF_Instance instance, RunParameters parameters continue; } - HighLevelNode reInsertionNode = this.explored.get(newConfiguration); + HighLevelNode reInsertionNode = null; + if (this.needToHandleConstraints) { + List nodesWithSameLocations = this.exploredWithExternalConstraints.get(newConfiguration); + if (nodesWithSameLocations != null) { + for (HighLevelNode node : nodesWithSameLocations) { + if (node.timeStep == this.timeStep) { + reInsertionNode = node; + } + } + } + } + else { + reInsertionNode = this.explored.get(newConfiguration); + } + if (!this.transientMAPFSettings.isTransientMAPF()) { if (reInsertionNode != null) { // re-insertion of already seen configuration @@ -253,7 +274,19 @@ protected void createNewHighLevelNode(HighLevelNode N, HashMap nodesList = this.exploredWithExternalConstraints.get(newConfiguration); + if (nodesList == null) { + nodesList = new ArrayList<>(); + this.exploredWithExternalConstraints.put(newConfiguration, nodesList); + } + nodesList.add(N_new); + this.explored.put(newConfiguration, N_new); + } + else { + this.explored.put(newConfiguration, N_new); + } } /** @@ -286,8 +319,7 @@ protected boolean reachedGoalConfiguration(HighLevelNode N) { for (Map.Entry entry : configuration.entrySet()) { Agent currentAgent = entry.getKey(); I_Location currentLocation = entry.getValue(); - I_Location goalLocation = this.goalConfiguration.get(currentAgent); - if (!(currentLocation.equals(goalLocation))) { + if (!(currentLocation.equals(getAgentsTarget(currentAgent)))) { return false; } } @@ -299,6 +331,16 @@ protected boolean reachedGoalConfiguration(HighLevelNode N) { } } } + + // final check - if all agents can stay at their current locations for infinite time + if (this.needToHandleConstraints) { + for (Agent agent : this.instance.agents) { + Move finalMove = getNewMove(agent, this.occupiedNextConfig.get(agent), this.occupiedNowConfig.get(agent)); + if (this.constraintsSet.lastRejectionTime(finalMove) != -1) { + return false; + } + } + } return true; } @@ -442,10 +484,17 @@ protected HashMap getNewConfig(HighLevelNode N, LowLevelNode this.occupiedNowConfig.put(agent, agentLocation); } - this.instanceConstraints = new ConstraintSet(this.solverConstraints); // bottom of low level tree - each agent have a constraint // exactly one configuration is possible if (C.depth == this.instance.agents.size()) { + + // check that low-level do not conflict with problem constraints. + if (this.needToHandleConstraints) { + Move newMove = getNewMove(C.who, C.where, this.occupiedNowConfig.get(C.who)); + if (!this.constraintsSet.accepts(newMove)) { + return null; + } + } while (C.parent != null) { this.occupiedNextConfig.put(C.who, C.where); C = C.parent; @@ -474,10 +523,15 @@ protected HashMap getNewConfig(HighLevelNode N, LowLevelNode return null; // Swap conflict detected! } } - Constraint swapConstraint = new Constraint(1, C.where, N.configuration.get(C.who)); - this.instanceConstraints.add(swapConstraint); - Constraint locationConstraint = new Constraint(1, C.where); - this.instanceConstraints.add(locationConstraint); + + // check that low-level does not conflict with problem constraints. + if (this.needToHandleConstraints) { + Move newMove = getNewMove(C.who, nextLocation, currentLocation); + if (!this.constraintsSet.accepts(newMove)) { + return null; + } + } + this.occupiedNextConfig.put(C.who, C.where); C = C.parent; } @@ -536,6 +590,13 @@ protected boolean solvePIBT(Agent currentAgent, @Nullable Agent higherPriorityAg if (nextLocation.equals(otherAgentLocation)) { // same agent, needs to stay in current location if (otherAgent.equals(currentAgent)) { + // check that the move does not conflict with problem constraints. + if (this.needToHandleConstraints) { + Move newMove = getNewMove(currentAgent, nextLocation, currentLocation); + if (!this.constraintsSet.accepts(newMove)) { + break; + } + } this.occupiedNextConfig.put(currentAgent, nextLocation); return true; } @@ -549,9 +610,11 @@ protected boolean solvePIBT(Agent currentAgent, @Nullable Agent higherPriorityAg if (occupyingAgent != null && this.occupiedNextConfig.get(occupyingAgent) != null && this.occupiedNextConfig.get(occupyingAgent).equals(currentLocation)) continue; // check constraints - Move newMove = new Move(currentAgent, this.timeStep, currentLocation, nextLocation); - if (!this.instanceConstraints.accepts(newMove)) { - continue; + if (this.needToHandleConstraints) { + Move newMove = getNewMove(currentAgent, nextLocation, currentLocation); + if (!this.constraintsSet.accepts(newMove)) { + continue; + } } // reserve next location @@ -572,6 +635,15 @@ protected boolean solvePIBT(Agent currentAgent, @Nullable Agent higherPriorityAg return false; } + @NotNull + private Move getNewMove(Agent currentAgent, I_Location nextLocation, I_Location currentLocation) { + return new Move(currentAgent, this.timeStep, currentLocation, nextLocation); + } + + public I_Location getAgentsTarget(Agent agent) { + return this.instance.map.getMapLocation(agent.target); + } + /** * helper function to find all neighbors of single agent * @param location to find his neighbors @@ -598,8 +670,12 @@ protected void releaseMemory() { super.releaseMemory(); this.open = null; this.explored = null; + this.exploredWithExternalConstraints = null; this.heuristic = null; - this.goalConfiguration = null; this.agents = null; + this.constraintsSet = null; + this.instance = null; + this.occupiedNowConfig = null; + this.occupiedNextConfig = null; } } diff --git a/src/test/java/BasicMAPF/Solvers/LaCAM/LaCAM_SolverTest.java b/src/test/java/BasicMAPF/Solvers/LaCAM/LaCAM_SolverTest.java index 28c0ae70..314cfc03 100644 --- a/src/test/java/BasicMAPF/Solvers/LaCAM/LaCAM_SolverTest.java +++ b/src/test/java/BasicMAPF/Solvers/LaCAM/LaCAM_SolverTest.java @@ -31,6 +31,8 @@ public class LaCAM_SolverTest { private final MAPF_Instance exampleInstance = new MAPF_Instance("exampleInstance", mapTwoWallsSmall, new Agent[]{agent00to02, agent02to00}); private final MAPF_Instance instanceEmptyEasy = new MAPF_Instance("instanceEmpty", mapEmpty, new Agent[]{agent33to12, agent04to00}); + + private final MAPF_Instance instanceEmptyThreeAgents = new MAPF_Instance("instanceEmpty", mapEmpty, new Agent[]{agent33to12, agent04to00, agent21to43}); private final MAPF_Instance instanceEmptyHarder = new MAPF_Instance("instanceEmpty", mapEmpty, new Agent[] {agent33to12, agent12to33, agent53to05, agent43to11, agent04to00, agent00to10, agent55to34, agent34to32, agent31to14, agent40to02}); @@ -85,8 +87,6 @@ void exampleTest() { @Test void emptyMapValidityWithEasyConstraint() { MAPF_Instance testInstance = instanceEmptyEasy; - I_Coordinate coor13 = new Coordinate_2D(1,3); - I_Coordinate coor02 = new Coordinate_2D(0,2); Constraint constraint1 = new Constraint(agent33to12, 2, mapEmpty.getMapLocation(coor13)); Constraint constraint2 = new Constraint(agent04to00, 2, mapEmpty.getMapLocation(coor02)); Constraint constraint3 = new Constraint(agent33to12, 4, mapEmpty.getMapLocation(coor12)); @@ -101,6 +101,20 @@ void emptyMapValidityWithEasyConstraint() { assertTrue(solved.solves(testInstance)); } + @Test + void emptyMapValidityWithInfiniteAndRegularConstraints() { + MAPF_Instance testInstance = instanceEmptyThreeAgents; + Constraint constraint1 = new Constraint(agent33to12, 1, mapEmpty.getMapLocation(coor23)); + Constraint constraint2 = new Constraint(agent04to00, 4, mapEmpty.getMapLocation(coor00)); + Constraint constraint3 = new Constraint(agent21to43, 12, mapEmpty.getMapLocation(coor43)); + ConstraintSet constraints = new ConstraintSet(); + constraints.add(constraint1); + constraints.add(constraint2); + constraints.add(constraint3); + Solution solved = LaCAM_Solver.solve(testInstance, new RunParametersBuilder().setTimeout(timeout).setConstraints(constraints).setInstanceReport(instanceReport).createRP()); + System.out.println(solved); + assertTrue(solved.solves(testInstance)); + } @Test void exampleTestLaCAMStar() { diff --git a/src/test/java/BasicMAPF/TestConstants/Agents.java b/src/test/java/BasicMAPF/TestConstants/Agents.java index 98ccb3bb..44b867ae 100644 --- a/src/test/java/BasicMAPF/TestConstants/Agents.java +++ b/src/test/java/BasicMAPF/TestConstants/Agents.java @@ -79,4 +79,5 @@ public class Agents { public final static Agent agent02to30 = new Agent(serialID++, coor02, coor30); public final static Agent agent04to30 = new Agent(serialID++, coor04, coor30); public final static Agent agent43to32 = new Agent(serialID++, coor43, coor32); + public final static Agent agent21to43 = new Agent(serialID++, coor21, coor43); }