public void setConflicts(HashSet <TimedMove> ID_CAT, HashSet_U <TimedMove> CBS_CAT)
{
TimedMove m2 = new TimedMove();
if (this.prevStep == null)
{
return;
}
for (int i = 0; i < allSteps.Length; i++)
{
m2.setup(allSteps[i].getX(), allSteps[i].getY(), Move.Direction.NO_DIRECTION, getDepth());
if (ID_CAT != null && ID_CAT.Contains(m2))
{
conflicts++;
}
if (CBS_CAT != null && CBS_CAT.Contains(m2))
{
conflicts++;
}
m2.direction = Move.getDirection(allSteps[i].getX(), allSteps[i].getY(), prevStep.allSteps[i].getX(), prevStep.allSteps[i].getY());
m2.setOppositeMove();
if (ID_CAT != null && ID_CAT.Contains(m2))
{
conflicts++;
}
if (CBS_CAT != null && CBS_CAT.Contains(m2))
{
conflicts++;
}
}
}
/// <summary>
/// Setup the relevant data structures for a run.
/// </summary>
public virtual void Setup(ProblemInstance problemInstance, int minDepth, Run runner)
{
minCAViolations = int.MaxValue;
passed = 0;
this.generatedHL = 1;
this.expandedHL = 1;
this.generatedLL = 0;
this.expandedLL = 0;
this.totalCost = Constants.TIMEOUT_COST;
// If there exists relevant previously solved subproblems - use their solution as a lower bound
if (problemInstance.parameters.ContainsKey(CostTreeSearch.PARENT_GROUP1_KEY))
{
costA = ((AgentsGroup)(problemInstance.parameters[CostTreeSearch.PARENT_GROUP1_KEY])).solutionCost;
costB = ((AgentsGroup)(problemInstance.parameters[CostTreeSearch.PARENT_GROUP2_KEY])).solutionCost;
sizeOfA = ((AgentsGroup)(problemInstance.parameters[CostTreeSearch.PARENT_GROUP1_KEY])).Size();
}
else
{
costA = problemInstance.m_vAgents[0].h;
costB = 0;
sizeOfA = 1;
}
this.problem = problemInstance;
this.runner = runner;
closedList = new HashSet <CostTreeNode>();
openList = new LinkedList <CostTreeNode>();
int[] costs = new int[problem.GetNumOfAgents()];
AgentState temp;
for (int i = 0; i < problem.GetNumOfAgents(); i++)
{
temp = problem.m_vAgents[i];
costs[i] = Math.Max(problem.GetSingleAgentOptimalCost(temp), minDepth);
}
openList.AddFirst(new CostTreeNode(costs));
this.initialHeuristics = openList.First.Value.costs.Sum();
// Store parameters used by Trevor's Independence Detection algorithm
if (problemInstance.parameters.ContainsKey(Trevor.MAXIMUM_COST_KEY))
{
this.maxCost = (int)(problemInstance.parameters[Trevor.MAXIMUM_COST_KEY]);
}
else
{
this.maxCost = -1;
}
if (problemInstance.parameters.ContainsKey(Trevor.CONFLICT_AVOIDANCE))
{
ID_CAT = ((HashSet <TimedMove>)problemInstance.parameters[Trevor.CONFLICT_AVOIDANCE]);
}
if (problemInstance.parameters.ContainsKey(CBS_LocalConflicts.INTERNAL_CAT))
{
CBS_CAT = ((HashSet_U <TimedMove>)problemInstance.parameters[CBS_LocalConflicts.INTERNAL_CAT]);
}
}
/// <summary>
/// Setup the relevant data structures for a run.
/// </summary>
public virtual void Setup(ProblemInstance problemInstance, int minDepth, Run runner)
{
this.instance = problemInstance;
this.runner = runner;
WorldState root = this.CreateSearchRoot(minDepth);
root.h = (int)this.heuristic.h(root); // g was already set in the constructor
this.openList.Add(root);
this.closedList.Add(root, root);
this.ClearPrivateStatistics();
this.generated++; // The root
this.totalCost = 0;
this.solutionDepth = -1;
this.numOfAgents = problemInstance.m_vAgents.Length;
// Store parameters used by Trevor's Independence Detection algorithm
if (problemInstance.parameters.ContainsKey(Trevor.MAXIMUM_COST_KEY))
{
this.maxCost = (int)(problemInstance.parameters[Trevor.MAXIMUM_COST_KEY]);
}
else
{
this.maxCost = int.MaxValue;
}
if (problemInstance.parameters.ContainsKey(Trevor.ILLEGAL_MOVES_KEY) &&
((HashSet <TimedMove>)problemInstance.parameters[Trevor.ILLEGAL_MOVES_KEY]).Count != 0)
{
this.illegalMoves = (HashSet <TimedMove>)(problemInstance.parameters[Trevor.ILLEGAL_MOVES_KEY]);
}
else
{
this.illegalMoves = null;
}
if (problemInstance.parameters.ContainsKey(CBS_LocalConflicts.CONSTRAINTS) &&
((HashSet_U <CbsConstraint>)problemInstance.parameters[CBS_LocalConflicts.CONSTRAINTS]).Count != 0)
{
this.constraintList = (HashSet_U <CbsConstraint>)problemInstance.parameters[CBS_LocalConflicts.CONSTRAINTS];
}
if (problemInstance.parameters.ContainsKey(CBS_LocalConflicts.MUST_CONSTRAINTS) &&
((List <CbsConstraint>)problemInstance.parameters[CBS_LocalConflicts.MUST_CONSTRAINTS]).Count != 0)
{
List <CbsConstraint> musts = (List <CbsConstraint>)problemInstance.parameters[CBS_LocalConflicts.MUST_CONSTRAINTS];
this.mustConstraints = new List <CbsConstraint> [musts.Max <CbsConstraint>(con => con.GetTimeStep()) + 1]; // To have index MAX, array needs MAX + 1 places.
foreach (CbsConstraint con in musts)
{
int timeStep = con.GetTimeStep();
if (this.mustConstraints[timeStep] == null)
{
this.mustConstraints[timeStep] = new List <CbsConstraint>();
}
this.mustConstraints[timeStep].Add(con);
}
}
}
private HashSet <MMStarConstraint> importCBSConstraintsToMMStarConstraints(HashSet_U <CFMCbsConstraint> constraints)
{
HashSet <MMStarConstraint> mConstraints = new HashSet <MMStarConstraint>();
foreach (CFMCbsConstraint constraint in constraints)
{
mConstraints.Add(new MMStarConstraint(constraint));
}
return(mConstraints);
}
/// <summary>
/// Setup the relevant data structures for a run.
/// </summary>
public virtual void Setup(ProblemInstance problemInstance, int minDepth, Run runner, int minCost)
{
this.instance = problemInstance;
this.runner = runner;
WorldState root = this.CreateSearchRoot(minDepth, minCost);
root.h = (int)this.heuristic.h(root); // g was already set in the constructor
this.openList.Add(root);
this.closedList.Add(root, root);
this.ClearPrivateStatistics();
this.generated++; // The root
this.totalCost = 0;
this.singleCosts = null;
this.solution = null;
this.singlePlans = null;
this.conflictCounts = null;
this.conflictTimes = null;
this.solutionDepth = -1;
this.numOfAgents = problemInstance.m_vAgents.Length;
// Store parameters used by IndependenceDetection's Independence Detection algorithm
if (problemInstance.parameters.ContainsKey("ID-max-cost"))
{
this.maxCost = (int)(problemInstance.parameters["ID-max-cost"]);
}
else
{
this.maxCost = int.MaxValue;
}
if (problemInstance.parameters.ContainsKey("ID - reserved") &&
((HashSet <TimedMove>)problemInstance.parameters["ID - reserved"]).Count != 0)
{
this.illegalMoves = (HashSet <TimedMove>)(problemInstance.parameters["ID - reserved"]);
}
else
{
this.illegalMoves = null;
}
if (problemInstance.parameters.ContainsKey(CBS.CONSTRAINTS) &&
((HashSet_U <CbsConstraint>)problemInstance.parameters[CBS.CONSTRAINTS]).Count != 0)
{
this.constraints = (HashSet_U <CbsConstraint>)problemInstance.parameters[CBS.CONSTRAINTS];
}
}
/// <summary>
/// Solves the entire node - finds a plan for every agent group.
/// Since this method is only called for the root of the constraint tree, every agent is in its own group.
/// </summary>
/// <param name="depthToReplan"></param>
/// <returns></returns>
public bool Solve()
{
this.totalCost = 0;
ProblemInstance problem = this.cbs.GetProblemInstance();
HashSet <CFMCbsConstraint> newConstraints = this.GetConstraints(); // Probably empty as this is probably the root of the CT.
// Constraints initiated with the problem instance
//var constraints = (HashSet_U<CbsConstraint>)problem.parameters[MAPF_CBS.CONSTRAINTS];
var constraints = new HashSet_U <CFMCbsConstraint>();
Dictionary <int, int> agentsWithConstraints = null;
if (constraints.Count != 0)
{
int maxConstraintTimeStep = constraints.Max <CFMCbsConstraint>(constraint => constraint.time);
agentsWithConstraints = constraints.Select <CFMCbsConstraint, int>(constraint => constraint.agentNum).Distinct().ToDictionary <int, int>(x => x); // ToDictionary because there's no ToSet...
}
constraints.Join(newConstraints);
// This mechanism of adding the constraints to the possibly pre-existing constraints allows having
// layers of CBS solvers, each one adding its own constraints and respecting those of the solvers above it.
// Solve using MMMStar
HashSet <MMStarConstraint> mConstraints = importCBSConstraintsToMMStarConstraints(constraints);
this.cbs.runner.SolveGivenProblem(problem, mConstraints);
this.mamPlan = this.cbs.runner.plan;
this.mamCost = this.cbs.runner.solutionCost;
// Gather conflicts
this.nodeConflicts = gatherConflicts();
//if(MAM_Run.toPrint)
// printConflicts(allSingleAgentPlans);
this.isGoal = this.nodeConflicts.Count == 0;
return(true);
}
public override LinkedList <Move>[] Solve(HashSet <TimedMove> conflictTable, HashSet_U <TimedMove> CBS_CAT)
{
for (int i = 0; i < allMDDs.Length; i++)
{
if (allMDDs[i].levels == null)
{
return(null);
}
}
AStarMDD findSolution = new AStarMDD(allMDDs, runner, conflictTable, CBS_CAT);
LinkedList <Move>[] ans = findSolution.Solve();
generated = findSolution.generated;
expanded = findSolution.expanded;
caViolations = findSolution.conflictAvoidanceViolations;
return(ans);
}
public override LinkedList <Move>[] Solve(HashSet <TimedMove> conflictTable, HashSet_U <TimedMove> CBS_CAT)
{
int notConflicting = 1;
for (int i = allMDDs.Length - 1; i >= 0; i--)
{
for (int j = i + 1; j < allMDDs.Length; j++)
{
if (syncSize == 2)
{
notConflicting = allMDDs[i].sync2GDDs(allMDDs[j]);
}
else if (syncSize == 3)
{
notConflicting = allMDDs[i].sync3GDDs(allMDDs[j], j);
}
//Run.resultsWriterdd.Write(matchCounter + ",");
//Run.resultsWriterdd.WriteLine();
if (notConflicting == 0)
{
return(null);
}
}
}
if (allMDDs[0].levels == null)
{
return(null);
}
CostTreeSearchSolver.passed++;
AStarMDD findSolution = new AStarMDD(allMDDs, runner, conflictTable, CBS_CAT);
LinkedList <Move>[] ans = findSolution.Solve();
generated = findSolution.generated;
expanded = findSolution.expanded;
caViolations = findSolution.conflictAvoidanceViolations;
return(ans);
}
public AStarMDD(MDD[] problem, Run runner, HashSet <TimedMove> conflicts, HashSet_U <TimedMove> CBS_CAT)
{
this.expanded = 0;
this.generated = 0;
MDDStep root;
this.problem = problem;
this.runner = runner;
this.ID_CAT = conflicts;
this.CBS_CAT = CBS_CAT;
this.closedList = new Dictionary <MDDStep, MDDStep>();
this.openList = new BinaryHeap();
MDDNode[] sRoot = new MDDNode[problem.Length];
for (int i = 0; i < problem.Length; i++)
{
sRoot[i] = problem[i].levels[0].First.Value;
sRoot[i].legal = true;
}
root = new MDDStep(sRoot, null);
openList.Add(root);
// Not adding it automatically to the closed list here?
conflictAvoidanceViolations = 0;
}
/// <summary>
///
/// </summary>
/// <param name="mddNum"></param>
/// <param name="agentNum"></param>
/// <param name="start_pos"></param>
/// <param name="cost">The MDD must be of this cost</param>
/// <param name="numOfLevels">
/// The MDD must be of this number of levels, not counting level zero.
/// If higher than cost, the extra levels will be WAITs at the goal.
/// </param>
/// <param name="numOfAgents"></param>
/// <param name="instance"></param>
/// <param name="ignoreConstraints"></param>
public MDD(int mddNum, int agentNum, Move start_pos, int cost, int numOfLevels, int numOfAgents, ProblemInstance instance, bool ignoreConstraints = false, bool supportPruning = true)
{
this.problem = instance;
this.mddNum = mddNum;
this.agentNum = agentNum;
this.cost = cost;
this.levels = new LinkedList <MDDNode> [numOfLevels + 1];
this.supportPruning = supportPruning;
if (ignoreConstraints == false && instance.parameters.ContainsKey(CBS_LocalConflicts.CONSTRAINTS) &&
((HashSet_U <CbsConstraint>)instance.parameters[CBS_LocalConflicts.CONSTRAINTS]).Count != 0)
{
this.queryConstraint = new CbsConstraint();
this.queryConstraint.queryInstance = true;
this.constraints = (HashSet_U <CbsConstraint>)instance.parameters[CBS_LocalConflicts.CONSTRAINTS];
}
if (ignoreConstraints == false && instance.parameters.ContainsKey(CBS_LocalConflicts.MUST_CONSTRAINTS) &&
((HashSet_U <CbsConstraint>)instance.parameters[CBS_LocalConflicts.MUST_CONSTRAINTS]).Count != 0)
{
// TODO: Code dup with ClassicAStar's constructor
var musts = (HashSet_U <CbsConstraint>)instance.parameters[CBS_LocalConflicts.MUST_CONSTRAINTS];
this.mustConstraints = new Dictionary <int, TimedMove> [musts.Max <CbsConstraint>(con => con.GetTimeStep()) + 1]; // To have index MAX, array needs MAX + 1 places.
foreach (CbsConstraint con in musts)
{
int timeStep = con.GetTimeStep();
if (this.mustConstraints[timeStep] == null)
{
this.mustConstraints[timeStep] = new Dictionary <int, TimedMove>();
}
this.mustConstraints[timeStep][con.agentNum] = con.move;
}
}
var closedList = new Dictionary <MDDNode, MDDNode>();
var toDelete = new List <MDDNode>();
for (int i = 0; i <= numOfLevels; i++)
{
levels[i] = new LinkedList <MDDNode>();
}
MDDNode root = new MDDNode(new TimedMove(start_pos, 0), numOfAgents, this, supportPruning); // Root
LinkedListNode <MDDNode> llNode = new LinkedListNode <MDDNode>(root);
root.setMyNode(llNode);
llNode.Value.startOrGoal = true;
levels[0].AddFirst(llNode);
for (int i = 0; i < numOfLevels; i++) // For each level, populate the _next_ level
{
int heuristicBound = cost - i - 1; // We want g+h <= cost, so h <= cost-g. -1 because it's the bound of the _children_.
if (heuristicBound < 0)
{
heuristicBound = 0;
}
// Go over each MDDNode in this level
foreach (MDDNode currentMddNode in levels[i]) // Since we're not deleting nodes in this method, we can use the simpler iteration method :)
{
List <MDDNode> children = this.GetAllChildren(currentMddNode, heuristicBound, numOfAgents);
if (children.Count == 0) // Heuristic wasn't perfect because of constraints, illegal moves or other reasons
{
toDelete.Add(currentMddNode);
}
foreach (MDDNode child in children)
{
MDDNode toAdd = child; // The compiler won't let me assign to the foreach variable...
if (closedList.ContainsKey(child))
{
toAdd = closedList[child];
}
else
{
closedList.Add(toAdd, toAdd);
llNode = new LinkedListNode <MDDNode>(toAdd);
toAdd.setMyNode(llNode);
levels[i + 1].AddLast(toAdd);
}
currentMddNode.addChild(toAdd); // forward edge
toAdd.addParent(currentMddNode); // backward edge
}
}
closedList.Clear();
}
foreach (MDDNode goal in levels[numOfLevels]) // The goal may be reached in more than one direction
{
goal.startOrGoal = true;
}
foreach (MDDNode remove in toDelete)
{
remove.delete();
}
// Make sure the goal was reached - imperfect heuristics, constraints or illegal moves can cause this to be false.
if (levels[numOfLevels].Count == 0 || levels[0].First.Value.isDeleted == true) //if no possible route mark levels as null
{
levels = null;
}
}
/// <summary> /// Tries to find a solution for the agents with the given cost. /// </summary> /// <returns>The solution if found or null otherwise</returns> public abstract LinkedList <Move>[] Solve(HashSet <TimedMove> conflictTable, HashSet_U <TimedMove> CBS_CAT);
public override LinkedList <Move>[] Solve(HashSet <TimedMove> conflictTable, HashSet_U <TimedMove> CBS_CAT)
{
MDD[] match = new MDD[2];
bool Converging = true;
int[] changed = new int[allMDDs.Length];
int currentIteration = 0;
int conflictStatus = 1;
while (Converging)
{
currentIteration++;
Converging = false;
for (int i = allMDDs.Length - 1; i >= 0; i--)
{
for (int j = i + 1; j < allMDDs.Length; j++)
{
if (changed[i] >= currentIteration - 1 || changed[j] >= currentIteration - 1)//if at least one of the two MDDs was changed during the last iteration
{
if (syncSize == 2)
{
conflictStatus = allMDDs[i].sync2GDDs(allMDDs[j]);
}
else if (syncSize == 3)
{
conflictStatus = allMDDs[i].sync3GDDs(allMDDs[j], j);
}
if (conflictStatus == 0)
{
return(null);
}
else if (conflictStatus == 2)
{
changed[i] = currentIteration;
Converging = true;
}
if (syncSize == 2)
{
conflictStatus = allMDDs[i].sync2GDDs(allMDDs[j]);
}
else if (syncSize == 3)
{
conflictStatus = allMDDs[i].sync3GDDs(allMDDs[j], j);
}
if (conflictStatus == 0)
{
return(null);
}
else if (conflictStatus == 2)
{
changed[i] = currentIteration;
Converging = true;
}
}
}
}
}
CostTreeSearchSolver.passed++;
if (allMDDs[0].levels == null)
{
return(null);
}
AStarMDD findSolution = new AStarMDD(allMDDs, runner, conflictTable, CBS_CAT);
LinkedList <Move>[] ans = findSolution.Solve();
generated = findSolution.generated;
expanded = findSolution.expanded;
caViolations = findSolution.conflictAvoidanceViolations;
return(ans);
}
public override LinkedList <Move>[] Solve(HashSet <TimedMove> conflictTable, HashSet_U <TimedMove> CBS_CAT)
{
AStarMDD findSolution;
LinkedList <Move>[] subCheck;
MDD[] match;
MddMatchAndPrune matcher = new MddMatchAndPrune(runner);
foreach (MDD checkValid in allMDDs)
{
if (checkValid.levels == null)
{
return(null);
}
}
if (maxGroupChecked >= 2)
{
match = new MDD[2];
for (int i = allMDDs.Length - 1; i >= 0; i--)
{
for (int j = i + 1; j < allMDDs.Length; j++)
{
match[0] = allMDDs[i];
match[1] = allMDDs[j];
//matcher.initialize(match);
//if (matcher.pruneMDDs() == false)
findSolution = new AStarMDD(match, runner, conflictTable, CBS_CAT);
subCheck = findSolution.Solve();
if (subCheck == null || subCheck[0] == null)
{
return(null);
}
}
}
}
if (maxGroupChecked >= 3)
{
match = new MDD[3];
for (int i = allMDDs.Length - 2; i >= 0; i--)
{
for (int j = i + 1; j < allMDDs.Length - 1; j++)
{
for (int t = j + 1; t < allMDDs.Length; t++)
{
match[0] = allMDDs[i];
match[1] = allMDDs[j];
match[2] = allMDDs[t];
//matcher.initialize(match);
//if (matcher.pruneMDDs() == false)
findSolution = new AStarMDD(match, runner, conflictTable, CBS_CAT);
subCheck = findSolution.Solve();
if (subCheck == null || subCheck[0] == null)
{
return(null);
}
}
}
}
}
if (maxGroupChecked >= 4)
{
match = new MDD[4];
for (int i = allMDDs.Length - 3; i >= 0; i--)
{
for (int j = i + 1; j < allMDDs.Length - 2; j++)
{
for (int t = j + 1; t < allMDDs.Length - 1; t++)
{
for (int m = t + 1; m < allMDDs.Length; m++)
{
match[0] = allMDDs[i];
match[1] = allMDDs[j];
match[2] = allMDDs[t];
match[3] = allMDDs[m];
//matcher.initialize(match);
//if (matcher.pruneMDDs() == false)
findSolution = new AStarMDD(match, runner, conflictTable, CBS_CAT);
subCheck = findSolution.Solve();
if (subCheck == null || subCheck[0] == null)
{
return(null);
}
}
}
}
}
}
CostTreeSearchSolver.passed++;
if (allMDDs[0].levels == null)
{
return(null);
}
findSolution = new AStarMDD(allMDDs, runner, conflictTable, CBS_CAT);
LinkedList <Move>[] ans = findSolution.Solve();
generated = findSolution.generated;
expanded = findSolution.expanded;
caViolations = findSolution.conflictAvoidanceViolations;
return(ans);
}