/// <summary>
/// Finds the first conflict (timewise) for all the given plans, or declares this node as a goal.
/// Assumes all agents are initially on the same timestep (no OD).
/// </summary>
private void FindConflict()
{
this.conflict = null;
if (this.allSingleAgentPlans.Length == 1)
{
return;
}
int maxPlanSize = this.allSingleAgentPlans.Max <SinglePlan>(plan => plan.GetSize());
// Check in every time step that the plans do not collide
for (int time = 1; time < maxPlanSize; time++)
{
// Check all pairs of groups if they are conflicting at the given time step
for (int i = 0; i < allSingleAgentPlans.Length; i++)
{
for (int j = i + 1; j < allSingleAgentPlans.Length; j++)
{
if (allSingleAgentPlans[i].IsColliding(time, allSingleAgentPlans[j]))
{
int initialTimeStep = this.problem.m_vAgents[0].lastMove.time; // To account for solving partially solved problems.
// This assumes the makespan of all the agents is the same.
Move first = allSingleAgentPlans[i].GetLocationAt(time);
Move second = allSingleAgentPlans[j].GetLocationAt(time);
this.conflict = new CbsConflict(i, j, first, second, time + initialTimeStep);
return;
}
}
}
}
}
///// <summary>
/////
///// </summary>
///// <param name="agentIndex"></param>
///// <param name="fromNode"></param>
///// <returns>Whether the shuffle succeeded</returns>
//bool RMStarShuffleIndividualPath(CbsConflict conflict, bool agentA, WorldState fromNode)
//{
// int agentIndex = agentA ? conflict.agentAIndex : conflict.agentBIndex;
// //WorldState node = fromNode.individualMStarPlanBases[agentIndex];
// WorldState node = fromNode;
// if (this.mstarPlanBasesToTheirConstraints.ContainsKey(node) == false)
// this.mstarPlanBasesToTheirConstraints[node] = new HashSet<CbsConstraint>[this.instance.GetNumOfAgents()];
// if (this.mstarPlanBasesToTheirConstraints[node][agentIndex] == null)
// this.mstarPlanBasesToTheirConstraints[node][agentIndex] = new HashSet<CbsConstraint>();
// return solveOneAgentForMstar(node, conflict, agentA);
//}
//protected bool solveOneAgentForMstar(WorldState node, CbsConflict conflict, bool agentA)
//{
// int agentIndex = agentA ? conflict.agentAIndex : conflict.agentBIndex;
// HashSet_U<CbsConstraint> constraints = null;
// HashSet<CbsConstraint> newConstraints = null;
// int oldMaxCost = int.MaxValue;
// if (this.instance.parameters.ContainsKey(CBS_LocalConflicts.CONSTRAINTS))
// constraints = (HashSet_U<CbsConstraint>)this.instance.parameters[CBS_LocalConflicts.CONSTRAINTS];
// else
// {
// constraints = new HashSet_U<CbsConstraint>();
// this.instance.parameters[CBS_LocalConflicts.CONSTRAINTS] = constraints;
// }
// if (this.instance.parameters.ContainsKey(CBS_LocalConflicts.CAT) == false)
// this.instance.parameters[CBS_LocalConflicts.CAT] = new Dictionary_U<TimedMove, int>(); // Indicate TO CBS that another level is running above it
// if (this.debug)
// {
// Debug.Print("Planning for agent index: " + agentIndex + " in node: " + node);
// }
// newConstraints = this.mstarPlanBasesToTheirConstraints[node][agentIndex];
// CbsConstraint newConstraint = new CbsConstraint(conflict, this.instance, agentA);
// newConstraints.Add(newConstraint);
// constraints.Join(newConstraints);
// if (this.debug)
// {
// Debug.Print("Constraints: ");
// foreach (var constraint in newConstraints)
// {
// Debug.Print(constraint.ToString());
// }
// }
// oldMaxCost = this.maxCost;
// //this.instance.parameters[IndependenceDetection.MAXIMUM_COST_KEY] = this.mstarPlanBasesToTheirPlans[node][agentIndex].GetSize() - 1;
// this.instance.parameters[IndependenceDetection.MAXIMUM_COST_KEY] = node.individualMStarPlans[agentIndex].GetSize() - 1;
// bool success = solveOneAgentForMstar(node, agentIndex);
// constraints.Separate(newConstraints);
// this.instance.parameters[IndependenceDetection.MAXIMUM_COST_KEY] = oldMaxCost;
// this.instance.parameters.Remove(CBS_LocalConflicts.CAT);
// return success;
//}
//protected bool solveOneAgentForMstar(WorldState node, int agentIndex)
//{
// AgentState[] thisAgentOnly = new AgentState[1];
// thisAgentOnly[0] = node.allAgentsState[agentIndex];
// var subProblem = this.instance.Subproblem(thisAgentOnly);
// ClassicAStar astar = new ClassicAStar(this.heuristic);
// ICbsSolver solver = new CBS_LocalConflicts(astar, astar); // Uses a precomputed solution if possible
// solver.Setup(subProblem, this.runner);
// bool success = solver.Solve();
// if (success)
// {
// //this.mstarPlanBasesToTheirPlans[node][agentIndex] = solver.GetSinglePlans()[0];
// node.individualMStarPlans[agentIndex] = solver.GetSinglePlans()[0];
// }
// // else nothing. Don't null the old plan yet - it might be saved by a successful replan of the other agent
// return success;
//}
/// <summary>
/// NOT the algorithm in the M* journal paper.
/// They want each node to propagate its entire collision set, not just the new conflict that began the process.
/// This implementation may be suitable for the M* algorithm as appears in the paper,
/// but it isn't suitable when we want to backpropagate from a closed list hit,
/// because then we don't have a specific conflict to propagate.
/// </summary>
/// <param name="conflict"></param>
/// <param name="fromNode">
/// Not the node where the collision happened, because it was never generated.
/// The node from where the colliding moves were made.
/// </param>
void RMStarCollisionBackPropagation(CbsConflict conflict, WorldState fromNode)
{
if (this.debug)
{
Debug.Print("Back prop!!");
}
var queue = new Queue <WorldState>();
queue.Enqueue(fromNode);
while (queue.Count != 0)
{
var node = queue.Dequeue();
bool onlyUnitedNow = node.collisionSets.Union(conflict.agentAIndex, conflict.agentBIndex);
if (onlyUnitedNow)
{
if (this.debug)
{
Console.WriteLine("Re-opening node {0} with an updated collision set", node);
}
this.reinsertIntoOpenList(node);
foreach (var next in node.backPropagationSet)
{
queue.Enqueue(next);
}
}
}
}
protected void addToGlobalConflictCount(CbsConflict conflict)
{
if (conflict != null)
{
globalConflictsCounter[Math.Max(conflict.agentA, conflict.agentB)][Math.Min(conflict.agentA, conflict.agentB)]++;
}
}
} // Nonsense values until Init, just allocate move
public CbsConstraint(CbsConflict conflict, ProblemInstance instance, bool agentA, Run.ConstraintPolicy constraintPolicy = Run.ConstraintPolicy.Single, int constraintRange = 0)
{
Move move;
int agentNum;
int minTime;
this.constaintRange = Math.Abs(conflict.timeStepAgentB - conflict.timeStepAgentA);
if (constraintPolicy == Run.ConstraintPolicy.Range)
{
minTime = Math.Min(conflict.timeStepAgentA, conflict.timeStepAgentB);
}
else if (constraintPolicy == Run.ConstraintPolicy.DoubleRange)
{
minTime = conflict.timeStepAgentA;
}
else
{
if (agentA)
{
minTime = conflict.timeStepAgentA;
}
else
{
minTime = conflict.timeStepAgentB;
}
}
if (agentA)
{
move = conflict.agentAmove;
agentNum = instance.m_vAgents[conflict.agentAIndex].agent.agentNum;
this.move = new TimedMove(move, minTime);
}
else
{
move = conflict.agentBmove;
agentNum = instance.m_vAgents[conflict.agentBIndex].agent.agentNum;
this.move = new TimedMove(move, minTime);
}
this.agentNum = (byte)agentNum;
if (conflict.vertex)
{
this.move.direction = Move.Direction.NO_DIRECTION;
}
}
} // Nonsense values until Init, just allocate move
public CbsConstraint(CbsConflict conflict, ProblemInstance instance, bool agentA)
{
Move move;
int agentNum;
int minTime;
if (agentA)
{
minTime = conflict.timeStepAgentA;
}
else
{
minTime = conflict.timeStepAgentB;
}
if (agentA)
{
move = conflict.agentAmove;
agentNum = instance.m_vAgents[conflict.agentAIndex].agent.agentNum;
this.move = new TimedMove(move, minTime);
}
else
{
move = conflict.agentBmove;
agentNum = instance.m_vAgents[conflict.agentBIndex].agent.agentNum;
this.move = new TimedMove(move, minTime);
}
this.agentNum = (byte)agentNum;
if (conflict.vertex)
{
this.move.direction = Move.Direction.NO_DIRECTION;
}
}
public virtual bool Expand(CbsNode node, CbsConflict conflict)
{
if (runner.ElapsedMilliseconds() > Constants.MAX_TIME)
{
return(false);
}
highLevelExpanded++;
if (conflict == null)
{
this.totalCost = node.totalCost;
this.goalNode = node;
this.solution = node.CalculateJointPlan();
this.Clear();
return(true);
}
CbsNode toAdd;
CbsConstraint con2 = new CbsConstraint(conflict, instance, false);
CbsConstraint con1 = new CbsConstraint(conflict, instance, true);
byte stepLength = 0;
if (conflict.vertex)
{
stepLength = 1;
}
bool ok1 = false, ok2 = false;
if (node.totalCost + conflict.timeStep + stepLength - node.PathLength(conflict.agentA) <= fBound)
{
ok1 = true;
if (node.DoesMustConstraintAllow(con1))
{
toAdd = new CbsNode(node, con1, conflict.agentA);
toAdd.SetMustConstraint(con2);
if (toAdd.Replan3b(conflict.agentA, Math.Max(minCost, conflict.timeStep)))
{
this.highLevelGenerated++;
if (toAdd.totalCost <= fBound)
{
if (Expand(toAdd, toAdd.GetConflict()))
{
return(true);
}
}
else if (toAdd.totalCost < nextF)
{
nextF = toAdd.totalCost;
}
}
}
}
if (node.totalCost + conflict.timeStep + stepLength - node.PathLength(conflict.agentB) <= fBound)
{
ok2 = true;
if (node.DoesMustConstraintAllow(con2))
{
toAdd = new CbsNode(node, con2, conflict.agentB);
toAdd.SetMustConstraint(con1);
if (toAdd.Replan3b(conflict.agentB, Math.Max(minCost, conflict.timeStep)))
{
this.highLevelGenerated++;
if (toAdd.totalCost <= fBound)
{
if (Expand(toAdd, toAdd.GetConflict()))
{
return(true);
}
}
else if (toAdd.totalCost < nextF)
{
nextF = toAdd.totalCost;
}
}
}
}
if (ok1 && ok2)
{
toAdd = new CbsNode(node, con1, conflict.agentA);
if (toAdd.Replan3b(conflict.agentA, Math.Max(minCost, conflict.timeStep)))
{
if (toAdd.totalCost <= fBound)
{
toAdd = new CbsNode(toAdd, con2, conflict.agentB);
if (toAdd.Replan(conflict.agentB, Math.Max(minCost, conflict.timeStep)))
// FIXME: Should this really use the regular Replan() or was this a typo?
{
this.highLevelGenerated++;
if (toAdd.totalCost <= fBound)
{
if (Expand(toAdd, toAdd.GetConflict()))
{
return(true);
}
}
else if (toAdd.totalCost < nextF)
{
nextF = toAdd.totalCost;
}
}
}
}
}
return(false);
}
protected virtual void addToGlobalConflictCount(CbsConflict conflict)
{
}
protected CbsNode ConstraintExpand(CbsNode node, bool doLeftChild, out int closedListHitChildCost)
{
CbsConflict conflict = node.GetConflict();
int conflictingAgentIndex = doLeftChild? conflict.agentAIndex : conflict.agentBIndex;
CbsNode.ExpansionState expansionsState = doLeftChild ? node.agentAExpansion : node.agentBExpansion;
CbsNode.ExpansionState otherChildExpansionsState = doLeftChild ? node.agentBExpansion : node.agentAExpansion;
string agentSide = doLeftChild? "left" : "right";
int planSize = node.allSingleAgentPlans[conflictingAgentIndex].GetSize();
int groupSize = node.GetGroupSize(conflictingAgentIndex);
closedListHitChildCost = -1;
if ((Constants.Variant == Constants.ProblemVariant.ORIG &&
expansionsState == CbsNode.ExpansionState.NOT_EXPANDED && conflict.vertex == true &&
conflict.timeStep >= node.allSingleAgentCosts[conflictingAgentIndex] && // TODO: Can't just check whether the node is at its goal - the plan may involve it passing through its goal and returning to it later because of preexisting constraints.
node.h < conflict.timeStep + 1 - node.allSingleAgentCosts[conflictingAgentIndex] && // Otherwise we won't be increasing its h and there would be no reason to delay expansion
groupSize == 1) || // Otherwise an agent in the group can be forced to take a longer route without increasing the group's cost because another agent would be able to take a shorter route.
(Constants.Variant == Constants.ProblemVariant.NEW &&
expansionsState == CbsNode.ExpansionState.NOT_EXPANDED && conflict.vertex == true &&
((conflict.timeStep > planSize - 1 && node.h < 2) ||
(conflict.timeStep == planSize - 1 && node.h < 1)) &&
groupSize == 1)) // Otherwise an agent in the group can be forced to take a longer route without increasing the group's cost because another agent would be able to take a shorter route.
// Conflict happens when or after the agent reaches its goal, and the agent is in a single-agent group.
// With multi-agent groups, banning the goal doesn't guarantee a higher cost solution,
// since if an agent is forced to take a longer route it may enable another agent in the group
// to take a shorter route, getting an alternative solution of the same cost
// The child would cost a lot because:
// A) All WAIT moves in the goal before leaving it now add to the g (if we're in the original problem variant).
// B) We force the low level to compute a path longer than the optimal,
// and with a bad suprise towards the end in the form of a constraint,
// so the low-level's SIC heuristic performs poorly.
// C) We're banning the GOAL from all directions (since this is a vertex conflict),
// so any alternative plan will at least cost 1 more.
// We're ignoring edge conflicts because they can only happen at the goal when reaching it,
// and aren't guaranteed to increase the cost because the goal can still be possibly reached from another edge.
{
if (otherChildExpansionsState == CbsNode.ExpansionState.DEFERRED)
{
throw new Exception("Unexpected: Expansion of both children deffered, but this is a vertex conflict so that means the targets for the two agents are equal, which is illegal");
}
if (debug)
{
Debug.WriteLine("Skipping " + agentSide + " child for now");
}
if (doLeftChild)
{
node.agentAExpansion = CbsNode.ExpansionState.DEFERRED;
}
else
{
node.agentBExpansion = CbsNode.ExpansionState.DEFERRED;
}
// Add the minimal delta in the child's cost:
// since we're banning the goal at conflict.timeStep, it must at least do conflict.timeStep+1 steps
if (Constants.Variant == Constants.ProblemVariant.ORIG)
{
node.h = (ushort)(conflict.timeStep + 1 - node.allSingleAgentCosts[conflictingAgentIndex]);
}
else if (Constants.Variant == Constants.ProblemVariant.NEW)
{
if (conflict.timeStep > planSize - 1) // Agent will need to step out and step in to the goal, at least
{
node.h = 2;
}
else // Conflict is just when agent enters the goal, it'll have to at least wait one timestep.
{
node.h = 1;
}
}
return(node);
}
else if (expansionsState != CbsNode.ExpansionState.EXPANDED)
// Agent expansion already skipped in the past or not forcing it from its goal - finally generate the child:
{
if (debug)
{
Debug.WriteLine("Generating " + agentSide + " child");
}
if (doLeftChild)
{
node.agentAExpansion = CbsNode.ExpansionState.EXPANDED;
}
else
{
node.agentBExpansion = CbsNode.ExpansionState.EXPANDED;
}
var newConstraint = new CbsConstraint(conflict, instance, doLeftChild);
CbsNode child = new CbsNode(node, newConstraint, conflictingAgentIndex);
if (closedList.ContainsKey(child) == false)
{
int minCost = -1;
//if (this.useMddHeuristic)
// minCost = node.GetGroupCost(conflictingAgentIndex) + 1;
bool success = child.Replan(conflictingAgentIndex, this.minDepth, null, -1, minCost); // The node takes the max between minDepth and the max time over all constraints.
if (success == false)
{
return(null); // A timeout probably occured
}
if (debug)
{
Debug.WriteLine("Child hash: " + child.GetHashCode());
Debug.WriteLine("Child cost: " + child.totalCost);
Debug.WriteLine("Child min ops to solve: " + child.minOpsToSolve);
Debug.WriteLine("Child num of agents that conflict: " + child.totalInternalAgentsThatConflict);
Debug.WriteLine("Child num of internal conflicts: " + child.totalConflictsBetweenInternalAgents);
Debug.WriteLine("");
}
if (child.totalCost < node.totalCost && groupSize == 1) // Catch the error early
{
child.Print();
Debug.WriteLine("Child plan: (cost {0})", child.allSingleAgentCosts[conflictingAgentIndex]);
child.allSingleAgentPlans[conflictingAgentIndex].PrintPlan();
Debug.WriteLine("Parent plan: (cost {0})", node.allSingleAgentCosts[conflictingAgentIndex]);
node.allSingleAgentPlans[conflictingAgentIndex].PrintPlan();
Debug.Assert(false, "Single agent node with lower cost than parent! " + child.totalCost + " < " + node.totalCost);
}
return(child);
}
else
{
this.closedListHits++;
closedListHitChildCost = this.closedList[child].totalCost;
if (debug)
{
Debug.WriteLine("Child already in closed list!");
}
}
}
else
{
if (debug)
{
Debug.WriteLine("Child already generated before");
}
}
return(null);
}
/// <summary>
///
/// </summary>
/// <param name="node"></param>
/// <param name="children"></param>
/// <param name="adoptBy">If not given, adoption is done by expanded node</param>
/// <returns>true if adopted - need to rerun this method, ignoring the returned children from this call, bacause adoption was performed</returns>
protected bool ExpandImpl(CbsNode node, out IList <CbsNode> children, out bool reinsertParent)
{
CbsConflict conflict = node.GetConflict();
children = new List <CbsNode>();
CbsNode child;
reinsertParent = false;
int closedListHitChildCost;
bool leftSameCost = false; // To quiet the compiler
bool rightSameCost = false;
// Generate left child:
child = ConstraintExpand(node, true, out closedListHitChildCost);
if (child != null)
{
if (child == node) // Expansion deferred
{
reinsertParent = true;
}
else // New child
{
children.Add(child);
leftSameCost = child.totalCost == node.totalCost;
}
}
else // A timeout occured, or the child was already in the closed list.
{
if (closedListHitChildCost != -1)
{
leftSameCost = closedListHitChildCost == node.totalCost;
}
}
if (runner.ElapsedMilliseconds() > Constants.MAX_TIME)
{
return(false);
}
// Generate right child:
child = ConstraintExpand(node, false, out closedListHitChildCost);
if (child != null)
{
if (child == node) // Expansion deferred
{
reinsertParent = true;
}
else // New child
{
children.Add(child);
rightSameCost = child.totalCost == node.totalCost;
}
}
else // A timeout occured, or the child was already in the closed list.
{
if (closedListHitChildCost != -1)
{
rightSameCost = closedListHitChildCost == node.totalCost;
}
}
return(false);
}
public virtual void Expand(CbsNode node)
{
CbsConflict conflict = node.GetConflict();
CbsNode child;
if (this.mergeThreshold != -1 && ShouldMerge(node))
{
child = new CbsNode(node, node.agentsGroupAssignment[conflict.agentA], node.agentsGroupAssignment[conflict.agentB]);
if (closedList.ContainsKey(child) == false) // We may have already merged these agents in the parent
{
if (debug)
{
Debug.WriteLine("Merging agents {0} and {1}", conflict.agentA, conflict.agentB);
}
closedList.Add(child, child);
bool success = child.Replan(conflict.agentA, this.minDepth); // or agentB. Doesn't matter - they're in the same group.
if (debug)
{
Debug.WriteLine("New cost: " + child.totalCost);
var constraints = child.GetConstraints();
Debug.WriteLine(constraints.Count + " Remaining internal constraints:");
foreach (CbsConstraint constraint in constraints)
{
Debug.WriteLine(constraint);
}
var externalConstraints = (HashSet_U <CbsConstraint>) this.instance.parameters[CBS_LocalConflicts.CONSTRAINTS];
Debug.WriteLine(externalConstraints.Count.ToString() + " external constraints: ");
foreach (CbsConstraint constraint in externalConstraints)
{
Debug.WriteLine(constraint);
}
Debug.WriteLine("New conflict: " + child.GetConflict());
Debug.Write("New agent group assignments: ");
for (int i = 0; i < child.agentsGroupAssignment.Length; i++)
{
Debug.Write(" " + child.agentsGroupAssignment[i]);
}
Debug.WriteLine("");
Debug.Write("Single agent costs: ");
for (int i = 0; i < child.allSingleAgentCosts.Length; i++)
{
Debug.Write(" " + child.allSingleAgentCosts[i]);
}
Debug.WriteLine("");
child.CalculateJointPlan().PrintPlan();
Debug.WriteLine("");
Debug.WriteLine("");
}
this.maxSizeGroup = Math.Max(this.maxSizeGroup, child.replanSize);
if (success == false) // A timeout probably occured
{
return;
}
if (node.totalCost <= maxCost) // FIXME: Code dup with other new node creations
{
openList.Add(child);
this.highLevelGenerated++;
this.addToGlobalConflictCount(child.GetConflict());
}
}
else
{
closedListHits++;
}
return;
}
// Expand node, possibly partially:
// Generate left child:
int agentAGroupSize = node.GetGroupSize(node.agentsGroupAssignment[conflict.agentA]);
if (node.agentAExpansion == CbsNode.ExpansionState.NOT_EXPANDED && conflict.vertex == true &&
conflict.timeStep >= node.allSingleAgentCosts[conflict.agentA] && // TODO: Consider checking directly whether at the time of the conflict the agent would be at its goal according to the node.singleAgentPlans. It may be more readable.
agentAGroupSize == 1)
// Conflict happens when or after agent A reaches its goal, and agent A is in a single agent group.
// With multi-agent groups, banning the goal doesn't guarantee a higher cost solution,
// since if an agent is forced to take a longer route it may enable another agent in the group
// to take a shorter route, getting an alternative solution of the same cost
// The left child would cost a lot because:
// A) All WAIT moves in the goal before leaving it now add to the g.
// B) We force the low level to compute a path longer than the optimal,
// and with a bad suprise towards the end in the form of a constraint,
// so the low-level's SIC heuristic performs poorly.
// C) We're banning the GOAL from all directions (since this is a vertex conflict),
// so any alternative plan will at least cost 1 more.
// We're ignoring edge conflicts because they can only happen at the goal when reaching it,
// and aren't guaranteed to increase the cost because the goal can still be possibly reached from another edge.
{
if (debug)
{
Debug.WriteLine("Skipping left child");
}
node.agentAExpansion = CbsNode.ExpansionState.DEFERRED;
int agentAOldCost = node.allSingleAgentCosts[conflict.agentA];
// Add the minimal delta in the child's cost:
// since we're banning the goal at conflict.timeStep, it must at least do conflict.timeStep+1 steps
node.totalCost += (ushort)(conflict.timeStep + 1 - agentAOldCost);
openList.Add(node); // Re-insert node into open list with higher cost, don't re-increment global conflict counts
this.partialExpansions++;
}
else if (node.agentAExpansion != CbsNode.ExpansionState.EXPANDED)
// Agent A expansion already skipped in the past or not forcing A from its goal - finally generate the child:
{
if (debug)
{
Debug.WriteLine("Generating left child");
}
var newConstraint = new CbsConstraint(conflict, instance, true);
child = new CbsNode(node, newConstraint, conflict.agentA);
if (closedList.ContainsKey(child) == false)
{
closedList.Add(child, child);
if (child.Replan(conflict.agentA, this.minDepth)) // The node takes the max between minDepth and the max time over all constraints.
{
if (child.totalCost < node.totalCost && agentAGroupSize == 1)
{
Debug.WriteLine("");
Debug.Write("Single agent costs: ");
for (int i = 0; i < child.allSingleAgentCosts.Length; i++)
{
Debug.Write(" " + child.allSingleAgentCosts[i]);
}
Debug.WriteLine("");
//Debug.WriteLine("Offending plan:");
//child.CalculateJointPlan().PrintPlan();
Debug.WriteLine("Chlid plan: (cost {0})", child.allSingleAgentCosts[conflict.agentA]);
child.allSingleAgentPlans[conflict.agentA].PrintPlan();
Debug.WriteLine("Parent plan: (cost {0})", node.allSingleAgentCosts[conflict.agentA]);
node.allSingleAgentPlans[conflict.agentA].PrintPlan();
Debug.Assert(false, "Single agent node with lower cost than parent! " + child.totalCost + " < " + node.totalCost);
}
if (child.totalCost <= this.maxCost)
{
openList.Add(child);
this.highLevelGenerated++;
addToGlobalConflictCount(child.GetConflict());
if (debug)
{
Debug.WriteLine("Child cost: " + child.totalCost);
Debug.WriteLine("Child hash: " + child.GetHashCode());
Debug.WriteLine("");
}
}
}
else // A timeout probably occured
{
return;
}
}
else
{
this.closedListHits++;
}
node.agentAExpansion = CbsNode.ExpansionState.EXPANDED;
}
// Generate right child:
int agentBGroupSize = node.GetGroupSize(node.agentsGroupAssignment[conflict.agentB]);
if (node.agentBExpansion == CbsNode.ExpansionState.NOT_EXPANDED && conflict.vertex == true &&
conflict.timeStep >= node.allSingleAgentCosts[conflict.agentB] &&
agentBGroupSize == 1) // Again, skip expansion
{
if (debug)
{
Debug.WriteLine("Skipping right child");
}
if (node.agentAExpansion == CbsNode.ExpansionState.DEFERRED)
{
throw new Exception("Unexpected: Expansion of both children differed, but this is a vertex conflict so that means the targets for the two agents are equal, which is illegal");
}
node.agentBExpansion = CbsNode.ExpansionState.DEFERRED;
int agentBOldCost = node.allSingleAgentCosts[conflict.agentB];
node.totalCost += (ushort)(conflict.timeStep + 1 - agentBOldCost);
openList.Add(node); // Re-insert node into open list with higher cost, don't re-increment global conflict counts
this.partialExpansions++;
// TODO: Code duplication with agentA. Make this into a function.
}
else if (node.agentBExpansion != CbsNode.ExpansionState.EXPANDED)
{
if (debug)
{
Debug.WriteLine("Generating right child");
}
var newConstraint = new CbsConstraint(conflict, instance, false);
child = new CbsNode(node, newConstraint, conflict.agentB);
if (closedList.ContainsKey(child) == false)
{
closedList.Add(child, child);
if (child.Replan(conflict.agentB, this.minDepth)) // The node takes the max between minDepth and the max time over all constraints.
{
if (child.totalCost < node.totalCost && node.agentAExpansion == CbsNode.ExpansionState.EXPANDED &&
agentBGroupSize == 1)
{
Debug.WriteLine("");
Debug.Write("Single agent costs: ");
for (int i = 0; i < child.allSingleAgentCosts.Length; i++)
{
Debug.Write(" " + child.allSingleAgentCosts[i]);
}
Debug.WriteLine("");
//Debug.WriteLine("Offending plan:");
//child.CalculateJointPlan().PrintPlan();
Debug.WriteLine("Chlid plan: (cost {0})", child.allSingleAgentCosts[conflict.agentB]);
child.allSingleAgentPlans[conflict.agentB].PrintPlan();
Debug.WriteLine("Parent plan: (cost {0})", node.allSingleAgentCosts[conflict.agentB]);
node.allSingleAgentPlans[conflict.agentB].PrintPlan();
Debug.Assert(false, "Single agent node with lower cost than parent! " + child.totalCost + " < " + node.totalCost);
}
if (child.totalCost <= this.maxCost)
{
openList.Add(child);
this.highLevelGenerated++;
addToGlobalConflictCount(child.GetConflict());
if (debug)
{
Debug.WriteLine("Child cost: " + child.totalCost);
Debug.WriteLine("Child hash: " + child.GetHashCode());
Debug.WriteLine("");
}
}
}
else // A timeout probably occured
{
return;
}
}
else
{
this.closedListHits++;
}
node.agentBExpansion = CbsNode.ExpansionState.EXPANDED;
}
}