public virtual void Expand(CbsNode node)
{
ushort parentCost = node.totalCost;
ushort parentH = node.h;
IList <CbsNode> children = null; // To quiet the compiler
bool reinsertParent = false; // To quiet the compiler
this.ExpandImpl(node, out children, out reinsertParent);
// Both children considered. None adopted. Add them to the open list, and re-insert the partially expanded parent too if necessary.
if (reinsertParent)
{
this.openList.Add(node); // Re-insert node into open list with higher cost, don't re-increment global conflict counts
}
foreach (var child in children)
{
closedList.Add(child, child);
if (child.totalCost == parentCost) // Total cost didn't increase (yet)
{
child.h = parentH;
}
if (child.totalCost <= this.maxCost)
{
this.highLevelGenerated++;
openList.Add(child);
}
}
}
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;
}
}