Add basic single agent fail policies (#33)

* visualize waypoints

* Refactor conflict avoidance tables and add removable

* Add basic single agent fail policies

src/main/java/BasicMAPF/Solvers/AStar/RunParameters_SAAStar.java

@@ -3,7 +3,7 @@
 import BasicMAPF.Instances.Maps.Coordinates.I_Coordinate;
 import BasicMAPF.Solvers.AStar.CostsAndHeuristics.AStarGAndH;
 import BasicMAPF.Solvers.AStar.GoalConditions.I_AStarGoalCondition;
-import BasicMAPF.Solvers.ConstraintsAndConflicts.ConflictManagement.I_ConflictAvoidanceTable;
+import BasicMAPF.Solvers.ConstraintsAndConflicts.ConflictManagement.ConflictAvoidance.I_ConflictAvoidanceTable;
 import Environment.Metrics.InstanceReport;
 import BasicMAPF.Solvers.ConstraintsAndConflicts.Constraint.ConstraintSet;
 import BasicMAPF.Solvers.RunParameters;

src/main/java/BasicMAPF/Solvers/AStar/SingleAgentAStar_Solver.java

@@ -10,7 +10,7 @@
 import BasicMAPF.Solvers.AStar.CostsAndHeuristics.UnitCostsAndManhattanDistance;
 import BasicMAPF.Solvers.AStar.GoalConditions.I_AStarGoalCondition;
 import BasicMAPF.Solvers.AStar.GoalConditions.SingleTargetCoordinateGoalCondition;
-import BasicMAPF.Solvers.ConstraintsAndConflicts.ConflictManagement.I_ConflictAvoidanceTable;
+import BasicMAPF.Solvers.ConstraintsAndConflicts.ConflictManagement.ConflictAvoidance.I_ConflictAvoidanceTable;
 import Environment.Metrics.InstanceReport;
 import BasicMAPF.Solvers.*;
 import BasicMAPF.Solvers.ConstraintsAndConflicts.Constraint.ConstraintSet;

src/main/java/BasicMAPF/Solvers/CBS/CBS_Solver.java

@@ -7,7 +7,7 @@
 import BasicMAPF.Solvers.ConstraintsAndConflicts.ConflictManagement.ConflictManager;
 import BasicMAPF.Solvers.ConstraintsAndConflicts.ConflictManagement.CorridorConflictManager;
 import BasicMAPF.Solvers.ConstraintsAndConflicts.ConflictManagement.I_ConflictManager;
-import BasicMAPF.Solvers.ConstraintsAndConflicts.ConflictManagement.SingleUseConflictAvoidanceTable;
+import BasicMAPF.Solvers.ConstraintsAndConflicts.ConflictManagement.ConflictAvoidance.SingleUseConflictAvoidanceTable;
 import BasicMAPF.Solvers.ConstraintsAndConflicts.Constraint.Constraint;
 import BasicMAPF.Solvers.ConstraintsAndConflicts.Constraint.ConstraintSet;
 import Environment.Metrics.InstanceReport;

src/main/java/BasicMAPF/Solvers/ConstraintsAndConflicts/ConflictManagement/ConflictAvoidance/A_ConflictAvoidanceTable.java

@@ -0,0 +1,151 @@
+package BasicMAPF.Solvers.ConstraintsAndConflicts.ConflictManagement.ConflictAvoidance;
+
+import BasicMAPF.Instances.Agent;
+import BasicMAPF.Instances.Maps.I_Location;
+import BasicMAPF.Solvers.ConstraintsAndConflicts.ConflictManagement.DataStructures.TimeLocation;
+import BasicMAPF.Solvers.Move;
+import BasicMAPF.Solvers.SingleAgentPlan;
+import org.jetbrains.annotations.NotNull;
+import org.jetbrains.annotations.Nullable;
+
+import java.util.*;
+
+public abstract class A_ConflictAvoidanceTable implements I_ConflictAvoidanceTable {
+    /**
+     * An instance of {@link TimeLocation} to be reused again and again when querying data structures, instead of
+     * creating thousands of single use instances.
+     */
+    private final TimeLocation reusableTimeLocation1 = new TimeLocation(0, null);
+    /**
+     * An instance of {@link TimeLocation} to be reused again and again when querying data structures, instead of
+     * creating thousands of single use instances.
+     */
+    private final TimeLocation reusableTimeLocation2 = new TimeLocation(0, null);
+
+    /**
+     * Maps time locations to agents that occupy them.
+     */
+    protected final Map<TimeLocation, List<Move>> regularOccupancies = new HashMap<>();
+
+    /**
+     * If set to true, will check for conflicts at goal locations (after time of reaching them)
+     */
+    public boolean checkGoals = true;
+    /**
+     * If set to true, will not count agents being together at their (shared) goal as a conflict.
+     */
+    public boolean sharedGoals = false;
+    /**
+     * If true, agents staying at their source (since the start) will not conflict
+     */
+    public boolean sharedSources = false;
+    public boolean removeOccupancyListsWhenEmptied;
+
+    /**
+     * Constructor
+     *
+     * @param plans                           plans to put into maps, so we can find conflicts with them. Will not be modified.
+     * @param excludedAgent                     an agent whose plan is to be ignored (optional).
+     */
+    public A_ConflictAvoidanceTable(@Nullable Iterable<? extends SingleAgentPlan> plans, @Nullable Agent excludedAgent) {
+        if(plans != null){
+            for (SingleAgentPlan plan: plans){
+                if(excludedAgent == null || ! excludedAgent.equals(plan.agent)){
+                    addPlan(plan);
+                }
+            }
+        }
+    }
+
+    public A_ConflictAvoidanceTable() {
+        this(null, null);
+    }
+
+    public void addAll(@NotNull Iterable<SingleAgentPlan> plans){
+        for (SingleAgentPlan plan : plans){
+            addPlan(plan);
+        }
+    }
+
+    public void addPlan(SingleAgentPlan plan){
+        for (Move move : plan){
+            TimeLocation from = new TimeLocation(move.timeNow - 1, move.prevLocation);
+            TimeLocation to = new TimeLocation(move.timeNow, move.currLocation);
+            addOccupancy(from, move);
+            addOccupancy(to, move);
+            if(move.timeNow == plan.getEndTime()){
+                addGoalOccupancy(move.currLocation, move);
+            }
+        }
+
+    }
+
+    private void addOccupancy(TimeLocation timeLocation, Move move){
+        List<Move> occupanciesAtTimeLocation = regularOccupancies.computeIfAbsent(timeLocation, tl -> new ArrayList<>());
+        occupanciesAtTimeLocation.add(move);
+    }
+    protected abstract void checkInitGoalOccupancies();
+
+    protected abstract void addGoalOccupancy(I_Location location, Move finalMove);
+
+
+    /**
+     * Returns the number of conflicts that would be created by the given move.
+     * @param move the move to check for conflicts
+     * @return the number of conflicts that would be created by the given move.
+     */
+    public int numConflicts(Move move) {
+        TimeLocation from = reusableTimeLocation1.setTo(move.timeNow - 1, move.prevLocation);
+
+        TimeLocation to = reusableTimeLocation2.setTo(move.timeNow, move.currLocation);
+
+        int numVertexConflicts = getNumVertexConflictsExcludingGoalConflicts(move, to);
+
+        if(checkGoals){
+            numVertexConflicts += getNumGoalConflicts(move, to);
+        }
+
+        // time locations of a move that would create a swapping conflict
+        TimeLocation reverseFrom = to;
+        reverseFrom.time -= 1;
+        TimeLocation reverseTo = from;
+        reverseTo.time += 1;
+
+        int numSwappingConflicts = getNumSwappingConflicts(reverseFrom, reverseTo);
+
+        return numVertexConflicts + numSwappingConflicts;
+    }
+
+    private int getNumSwappingConflicts(TimeLocation reverseFrom, TimeLocation reverseTo) {
+        int numSwappingConflicts = 0;
+        if(regularOccupancies.containsKey(reverseFrom) && regularOccupancies.containsKey(reverseTo)){
+            // so there are occupancies at the times + locations of interest, now check if they are from a move from
+            // reverseFrom to reverseTo
+            for(Move fromMove : regularOccupancies.get(reverseFrom)){
+                if (fromMove.currLocation.equals(reverseTo.location)){
+                    numSwappingConflicts++;
+                }
+            }
+        }
+        return numSwappingConflicts;
+    }
+
+    private int getNumVertexConflictsExcludingGoalConflicts(Move move, TimeLocation to) {
+        int numVertexConflicts = 0;
+        if(regularOccupancies.containsKey(to)){
+            if (sharedSources && move.isStayAtSource){
+                // count conflicts excluding stay at source
+                for (Move otherMove : regularOccupancies.get(to)){
+                    numVertexConflicts += otherMove.isStayAtSource ? 1 : 0; //will only be same source
+                }
+            }
+            else {
+                numVertexConflicts += regularOccupancies.get(to).size();
+            }
+        }
+        return numVertexConflicts;
+    }
+
+    abstract int getNumGoalConflicts(Move move, TimeLocation to);
+}
+

src/main/java/BasicMAPF/Solvers/ConstraintsAndConflicts/ConflictManagement/I_ConflictAvoidanceTable.java → src/main/java/BasicMAPF/Solvers/ConstraintsAndConflicts/ConflictManagement/ConflictAvoidance/I_ConflictAvoidanceTable.java

@@ -1,4 +1,4 @@
-package BasicMAPF.Solvers.ConstraintsAndConflicts.ConflictManagement;
+package BasicMAPF.Solvers.ConstraintsAndConflicts.ConflictManagement.ConflictAvoidance;
 
 import BasicMAPF.Solvers.Move;
 

src/main/java/BasicMAPF/Solvers/ConstraintsAndConflicts/ConflictManagement/ConflictAvoidance/RemovableConflictAvoidanceTableWithContestedGoals.java

@@ -0,0 +1,138 @@
+package BasicMAPF.Solvers.ConstraintsAndConflicts.ConflictManagement.ConflictAvoidance;
+
+import BasicMAPF.Instances.Agent;
+import BasicMAPF.Instances.Maps.I_Location;
+import BasicMAPF.Solvers.ConstraintsAndConflicts.ConflictManagement.DataStructures.AgentAtGoal;
+import BasicMAPF.Solvers.ConstraintsAndConflicts.ConflictManagement.DataStructures.TimeLocation;
+import BasicMAPF.Solvers.Move;
+import BasicMAPF.Solvers.SingleAgentPlan;
+import org.jetbrains.annotations.Nullable;
+
+import java.util.*;
+
+/**
+ * Supports removing plans, and multiple goal occupancies on the same location.
+ * A plan for an agent should always be removed before adding a new plan for that agent.
+ */
+public class RemovableConflictAvoidanceTableWithContestedGoals extends A_ConflictAvoidanceTable {
+
+    /**
+     * An instance of {@link TimeLocation} to be reused again and again when querying data structures, instead of
+     * creating thousands of single use instances.
+     */
+    private final TimeLocation reusableTimeLocation1 = new TimeLocation(0, null);
+    /**
+     * An instance of {@link TimeLocation} to be reused again and again when querying data structures, instead of
+     * creating thousands of single use instances.
+     */
+    private final TimeLocation reusableTimeLocation2 = new TimeLocation(0, null);
+    private final AgentAtGoal reusableAgentAtGoal = new AgentAtGoal(null, 0);
+    Set<Agent> coveredAgents;
+    /**
+     * Contains all goal locations and maps them to the times from which they are occupied (indefinitely) and the agents that occupy them..
+     */
+    private Map<I_Location, List<AgentAtGoal>> goalOccupancies = new HashMap<>();
+
+    /**
+     * {@inheritDoc}
+     */
+    public RemovableConflictAvoidanceTableWithContestedGoals(@Nullable Iterable<? extends SingleAgentPlan> plans, @Nullable Agent excludedAgent) {
+        super(plans, excludedAgent);
+    }
+
+    public RemovableConflictAvoidanceTableWithContestedGoals() {
+        super();
+    }
+
+    /**
+     * Adds a plan to the table. If the agent already has a plan in the table, will throw an exception.
+     * @param plan the plan to add
+     */
+    @Override
+    public void addPlan(SingleAgentPlan plan) {
+        super.addPlan(plan);
+        if (coveredAgents == null){
+            coveredAgents = new HashSet<>();
+        }
+        if (coveredAgents.contains(plan.agent)){
+            throw new IllegalStateException("Agent " + plan.agent + " already has a plan in the table");
+        }
+        coveredAgents.add(plan.agent);
+    }
+
+    @Override
+    protected void checkInitGoalOccupancies() {
+        if (this.goalOccupancies == null){
+            this.goalOccupancies = new HashMap<>();
+        }
+    }
+
+    @Override
+    protected void addGoalOccupancy(I_Location location, Move finalMove) {
+        checkInitGoalOccupancies();
+        List<AgentAtGoal> agentsAtGoal = goalOccupancies.computeIfAbsent(location, k -> new ArrayList<>());
+        // add 1 to time so as not to overlap with the vertex conflict
+        agentsAtGoal.add(new AgentAtGoal(finalMove.agent, finalMove.timeNow + 1));
+    }
+
+    @Override
+    int getNumGoalConflicts(Move move, TimeLocation to) {
+        checkInitGoalOccupancies();
+        List<AgentAtGoal> agentsAtGoal = goalOccupancies.get(move.currLocation);
+        if (agentsAtGoal == null) {
+            return 0;
+        }
+        int numConflicts = 0;
+        for (AgentAtGoal agentAtGoal : agentsAtGoal) { // TODO more efficient with sorted list?
+            if (agentAtGoal.time <= to.time) {
+                numConflicts++;
+            }
+        }
+        return numConflicts;
+    }
+
+    /**
+     * Removes a plan from the table. Should be called before adding a new plan for the same agent.
+     * @param plan the plan to remove
+     */
+    public void removePlan(SingleAgentPlan plan){
+        for (Move move : plan){
+            TimeLocation from = reusableTimeLocation1.setTo(move.timeNow - 1, move.prevLocation);
+            TimeLocation to = reusableTimeLocation2.setTo(move.timeNow, move.currLocation);
+            removeOccupancy(from, move);
+            removeOccupancy(to, move);
+            if(move.timeNow == plan.getEndTime()){
+                removeGoalOccupancy(move.currLocation, move);
+            }
+        }
+
+        if (coveredAgents != null){
+            coveredAgents.remove(plan.agent); // TODO log a warning if the agent was not in the table?
+        }
+    }
+
+    private void removeOccupancy(TimeLocation timeLocation, Move move) {
+        List<Move> occupanciesAtTimeLocation = regularOccupancies.get(timeLocation);
+        if(occupanciesAtTimeLocation != null){
+            occupanciesAtTimeLocation.remove(move);
+            if(removeOccupancyListsWhenEmptied && occupanciesAtTimeLocation.isEmpty()){
+                regularOccupancies.remove(timeLocation);
+            }
+        }
+    }
+
+    private void removeGoalOccupancy(I_Location currLocation, Move move) {
+        List<AgentAtGoal> agentsAtGoal = goalOccupancies.get(currLocation);
+        if(agentsAtGoal != null){
+            agentsAtGoal.remove(reusableAgentAtGoal.setTo(move.agent, move.timeNow));
+            if(removeOccupancyListsWhenEmptied && agentsAtGoal.isEmpty()){
+                goalOccupancies.remove(currLocation);
+            }
+        }
+    }
+
+    public void replacePlan(SingleAgentPlan plan, SingleAgentPlan newPlan) {
+        removePlan(plan);
+        addPlan(newPlan);
+    }
+}

src/main/java/BasicMAPF/Solvers/ConstraintsAndConflicts/ConflictManagement/ConflictAvoidance/SingleUseConflictAvoidanceTable.java

@@ -0,0 +1,71 @@
+package BasicMAPF.Solvers.ConstraintsAndConflicts.ConflictManagement.ConflictAvoidance;
+
+import BasicMAPF.Instances.Agent;
+import BasicMAPF.Instances.Maps.I_Location;
+import BasicMAPF.Solvers.ConstraintsAndConflicts.ConflictManagement.DataStructures.TimeLocation;
+import BasicMAPF.Solvers.Move;
+import BasicMAPF.Solvers.SingleAgentPlan;
+import org.jetbrains.annotations.Nullable;
+
+import java.util.*;
+
+/**
+ * A conflict avoidance table that is meant to be used in a single CT node. Meant to be filled once with plans, queried
+ * many times about conflicts with those plans, and then be discarded.
+ */
+public class SingleUseConflictAvoidanceTable extends A_ConflictAvoidanceTable {
+    /**
+     * Contains all goal locations and maps them to the time from which they are occupied (indefinitely).
+     * Can't have more than one agent occupying a goal, since that would make the problem unsolvable (in classic MAPF).
+     */
+    private Map<I_Location, Integer> goalOccupancies;
+
+    /**
+     * {@inheritDoc}
+     */
+    public SingleUseConflictAvoidanceTable(@Nullable Iterable<? extends SingleAgentPlan> plans, @Nullable Agent excludedAgent) {
+        super(plans, excludedAgent);
+    }
+
+    /**
+     * {@inheritDoc}
+     */
+    public SingleUseConflictAvoidanceTable() {
+        super();
+    }
+
+    @Override
+    protected void checkInitGoalOccupancies() {
+        if (goalOccupancies == null){
+            this.goalOccupancies = new HashMap<>();
+        }
+    }
+
+    @Override
+    protected void addGoalOccupancy(I_Location location, Move finalMove){
+        checkInitGoalOccupancies();
+        // add 1 to entry time, so as not to count twice with the entry and in allOccupancies, and also not miss the
+        // possible swapping conflict on the last move in the plan (if we were to instead remove the last from allOccupancies)
+        if(checkGoals){
+            this.goalOccupancies.put(location, finalMove.timeNow + 1);
+        }
+    }
+
+    @Override
+    protected int getNumGoalConflicts(Move move, TimeLocation to){
+        checkInitGoalOccupancies();
+
+        int numGoalConflicts = 0;
+        // check for a goal occupancy conflicting with this move
+        if(goalOccupancies.containsKey(to.location) && goalOccupancies.get(to.location) <= to.time){
+            if (!(sharedGoals && move.currLocation.getCoordinate().equals(move.agent.target))){
+                numGoalConflicts++;
+            }
+        }
+        // doesn't check if this is a goal move and how many conflicts that would create, which should be OK since
+        // the number would be determined by the start time of staying in goal, and this method is used for tie-breaking
+        // between equal length plans, so staying at goal at a different time would be a different length plan anyway
+        return numGoalConflicts;
+    }
+
+}