/// <summary>
///
/// </summary>
/// <param name="problemInstance"></param>
/// <param name="minDepth"></param>
/// <param name="runner"></param>
/// <param name="minCost">Not taken into account</param>
public virtual void Setup(ProblemInstance problemInstance, int minDepth, Run runner, int minCost = -1)
{
this.instance = problemInstance;
this.runner = runner;
this.ClearPrivateStatistics();
this.totalCost = 0;
this.solutionDepth = -1;
this.targetCost = int.MaxValue;
this.lowLevelGeneratedCap = int.MaxValue;
this.milliCap = int.MaxValue;
this.goalNode = null;
this.solution = null;
this.maxCost = int.MaxValue;
this.topMost = this.SetGlobals();
this.minDepth = minDepth;
CbsNode root = new CbsNode(instance.m_vAgents.Length, this.solver, this.singleAgentSolver, this); // Problem instance and various strategy data is all passed under 'this'.
// Solve the root node
bool solved = root.Solve(minDepth);
if (solved && root.totalCost <= this.maxCost)
{
this.openList.Add(root);
this.highLevelGenerated++;
this.closedList.Add(root, root);
}
}
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 bool DoesMustConstraintAllow(CbsConstraint check)
{
CbsNode current = this;
while (current != null)
{
if (current.mustConstraint != null && !current.mustConstraint.Allows(check))
{
return(false);
}
current = current.prev;
}
return(true);
}
public int CompareTo(IBinaryHeapItem item)
{
CbsNode other = (CbsNode)item;
if (this.totalCost < other.totalCost)
{
return(-1);
}
if (this.totalCost > other.totalCost)
{
return(1);
}
// Tie breaking:
// We could prefer less external conflicts, even over goal nodes, as goal nodes with less external conflicts are better.
// External conflicts are already taken into account by the low level solver to prefer less conflicts between fewer agents.
// This would only help when this CBS is used as a low level solver, but is very costly to compute, and is computed also
// for high level CBS solvers, so I removed it.
// Internal conflict counts are ignored. 100 conflicts between the same two agents can possibly be solved by a single replan,
// while two conflicts between two sets of agents take two replans to solve.
// Prefer goal nodes. The elaborate form is to keep the comparison consistent. Without it goalA<goalB and also goalB<goalA.
if (this.GoalTest() == true && other.GoalTest() == false)
{
return(-1);
}
if (other.GoalTest() == true && this.GoalTest() == false)
{
return(1);
}
// Not preferring more depth because it makes no sense. It's not like preferring larger g,
// which is smart because that part of the cost isn't an estimate.
// Prefer partially expanded nodes. They're less work because they have less constraints and only one child to generate.
// The elaborate form, again, is to keep the comparison consistent. Without it partiallyExpandedA<partiallyExpandedB and partiallyExpandedA>partiallyExpandedB
if ((this.agentAExpansion == CbsNode.ExpansionState.DEFERRED || this.agentBExpansion == CbsNode.ExpansionState.DEFERRED) &&
other.agentAExpansion == CbsNode.ExpansionState.NOT_EXPANDED && other.agentBExpansion == CbsNode.ExpansionState.NOT_EXPANDED)
{
return(-1);
}
if ((other.agentAExpansion == CbsNode.ExpansionState.DEFERRED || other.agentBExpansion == CbsNode.ExpansionState.DEFERRED) &&
this.agentAExpansion == CbsNode.ExpansionState.NOT_EXPANDED && this.agentBExpansion == CbsNode.ExpansionState.NOT_EXPANDED)
{
return(1);
}
return(0);
}
public List <CbsConstraint> GetMustConstraints()
{
var constraints = new List <CbsConstraint>();
CbsNode current = this;
while (current.depth > 0)
{
if (current.mustConstraint != null)
{
constraints.Add(current.mustConstraint);
}
current = current.prev;
}
constraints.Sort();
return(constraints);
}
/// <summary>
/// Child from branch action constructor
/// </summary>
/// <param name="father"></param>
/// <param name="newConstraint"></param>
/// <param name="agentToReplan"></param>
public CbsNode(CbsNode father, CbsConstraint newConstraint, int agentToReplan)
{
this.allSingleAgentPlans = father.allSingleAgentPlans.ToArray <SinglePlan>();
this.allSingleAgentCosts = father.allSingleAgentCosts.ToArray <int>();
this.agentsGroupAssignment = father.agentsGroupAssignment.ToArray <ushort>();
this.prev = father;
this.constraint = newConstraint;
this.depth = (ushort)(this.prev.depth + 1);
agentAExpansion = ExpansionState.NOT_EXPANDED;
agentBExpansion = ExpansionState.NOT_EXPANDED;
replanSize = 1;
this.problem = father.problem;
this.solver = father.solver;
this.singleAgentSolver = father.singleAgentSolver;
this.runner = father.runner;
}
public void Setup(ProblemInstance problemInstance, Run runner)
{
AgentState.EquivalenceOverDifferentTimes = false;
globalConflictsCounter = new int[problemInstance.m_vAgents.Length][];
for (int i = 0; i < globalConflictsCounter.Length; i++)
{
globalConflictsCounter[i] = new int[i];
for (int j = 0; j < i; j++)
{
globalConflictsCounter[i][j] = 0;
}
}
this.instance = problemInstance;
this.runner = runner;
root = new CbsNode(instance.m_vAgents.Length, problemInstance, this.solver, this.lowLevelSolver, runner);
this.highLevelExpanded = 0;
this.highLevelGenerated = 1;
maxSizeGroup = 1;
this.totalCost = 0;
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(CBS_LocalConflicts.INTERNAL_CAT) == false) // Top-most CBS only
{
problemInstance.parameters[CBS_LocalConflicts.INTERNAL_CAT] = new HashSet_U <TimedMove>();
problemInstance.parameters[CBS_LocalConflicts.CONSTRAINTS] = new HashSet_U <CbsConstraint>();
problemInstance.parameters[CBS_LocalConflicts.MUST_CONSTRAINTS] = new List <CbsConstraint>();
this.topMost = true;
}
else
{
this.topMost = false;
}
minCost = 0;
}
public CbsNode(int numberOfAgents, ProblemInstance problem, ICbsSolver solver, ICbsSolver singleAgentSolver, Run runner)
{
allSingleAgentPlans = new SinglePlan[numberOfAgents];
allSingleAgentCosts = new int[numberOfAgents];
depth = 0;
replanSize = 1;
agentAExpansion = ExpansionState.NOT_EXPANDED;
agentBExpansion = ExpansionState.NOT_EXPANDED;
agentsGroupAssignment = new ushort[numberOfAgents];
for (ushort i = 0; i < numberOfAgents; i++)
{
agentsGroupAssignment[i] = i;
}
this.prev = null;
this.constraint = null;
this.problem = problem;
this.solver = solver;
this.singleAgentSolver = singleAgentSolver;
this.runner = runner;
}
public HashSet <CbsConstraint> GetConstraints()
{
var constraints = new HashSet <CbsConstraint>();
CbsNode current = this;
while (current.depth > 0) // The root has no constraints
{
if (this.agentsGroupAssignment[current.prev.conflict.agentA] !=
this.agentsGroupAssignment[current.prev.conflict.agentB]) // Ignore constraints that deal with conflicts between
// agents that were later merged. They're irrelevant
// since merging fixes all conflicts between merged agents.
// Nodes that only differ in such irrelevant conflicts will have the same single agent paths.
// Dereferencing current.prev is safe because current isn't the root.
{
constraints.Add(current.constraint);
}
current = current.prev;
}
return(constraints);
}
public virtual void Setup(ProblemInstance problemInstance, int minDepth, Run runner)
{
this.instance = problemInstance;
this.runner = runner;
this.ClearPrivateStatistics();
this.totalCost = 0;
this.solutionDepth = -1;
this.targetCost = int.MaxValue;
this.lowLevelGeneratedCap = int.MaxValue;
this.milliCap = int.MaxValue;
this.goalNode = null;
this.solution = null;
if (problemInstance.parameters.ContainsKey(Trevor.MAXIMUM_COST_KEY))
{
this.maxCost = (int)(problemInstance.parameters[Trevor.MAXIMUM_COST_KEY]);
}
else
{
this.maxCost = int.MaxValue;
}
this.topMost = this.SetGlobals();
this.minDepth = minDepth;
CbsNode root = new CbsNode(instance.m_vAgents.Length, problemInstance, this.solver, this.singleAgentSolver, runner);
// Solve the root node
bool solved = root.Solve(minDepth);
if (solved && root.totalCost <= this.maxCost)
{
this.openList.Add(root);
this.highLevelGenerated++;
this.closedList.Add(root, root);
this.addToGlobalConflictCount(root.GetConflict());
}
}
/// <summary>
/// Merge agent groups that are conflicting in this node if they pass the merge threshold.
/// </summary>
/// <param name="mergeThreshold"></param>
/// <returns>Whether a merge was performed.</returns>
public bool ShouldMerge(int mergeThreshold)
{
int countConflicts = 1; // The agentA and agentB conflict in this node.
ISet <int> firstGroup = this.GetGroup(this.agentsGroupAssignment[this.conflict.agentA]);
ISet <int> secondGroup = this.GetGroup(this.agentsGroupAssignment[this.conflict.agentB]);
CbsNode current = this.prev;
int a, b;
while (current != null)
{
a = current.conflict.agentA;
b = current.conflict.agentB;
if ((firstGroup.Contains(a) && secondGroup.Contains(b)) || (firstGroup.Contains(b) && secondGroup.Contains(a)))
{
countConflicts++;
}
current = current.prev;
}
return(countConflicts > mergeThreshold);
}
/// <summary>
/// Checks the group assignment and the constraints
/// </summary>
/// <param name="obj"></param>
/// <returns></returns>
public override bool Equals(object obj)
{
CbsNode other = (CbsNode)obj;
if (this.agentsGroupAssignment.SequenceEqual <ushort>(other.agentsGroupAssignment) == false)
{
return(false);
}
CbsNode current = this;
HashSet <CbsConstraint> other_constraints = other.GetConstraints();
HashSet <CbsConstraint> constraints = this.GetConstraints();
foreach (CbsConstraint constraint in constraints)
{
if (other_constraints.Contains(constraint) == false)
{
return(false);
}
current = current.prev;
}
return(constraints.Count == other_constraints.Count);
}
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);
}
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;
}
}
protected override bool ShouldMerge(CbsNode node)
{
return(node.ShouldMerge(mergeThreshold, globalConflictsCounter));
}
public bool Solve()
{
//this.SetGlobals(); // Again, because we might be resuming a search that was stopped.
int initialEstimate = 0;
if (openList.Count > 0)
{
initialEstimate = ((CbsNode)openList.Peek()).totalCost;
}
int maxExpandedNodeCostPlusH = -1;
int currentCost = -1;
while (openList.Count > 0)
{
// Check if max time has been exceeded
if (runner.ElapsedMilliseconds() > Constants.MAX_TIME)
{
this.totalCost = Constants.TIMEOUT_COST;
Console.WriteLine("Out of time");
this.solutionDepth = ((CbsNode)openList.Peek()).totalCost - initialEstimate; // A minimum estimate
this.Clear(); // Total search time exceeded - we're not going to resume this search.
this.CleanGlobals();
return(false);
}
var currentNode = (CbsNode)openList.Remove();
currentNode.ChooseConflict();
// A cardinal conflict may have been found, increasing the h of the node.
// Check if the node needs to be pushed back into the open list.
if (this.openList.Count != 0 &&
currentNode.f > ((CbsNode)this.openList.Peek()).f)
{
if (this.debug)
{
Debug.Print("Pushing back the node into the open list with an increased h.");
}
this.openList.Add(currentNode);
this.nodesPushedBack++;
continue;
// Notice that even though we may iterate over conflicts later,
// even there is a conflict that we can identify as cardinal,
// then the first conflict chosen _will_ be cardinal, so this is
// the only place we need allow pushing nodes back.
// We may discover cardinal conflicts in hindsight later, but there
// would be no point in pushing their node back at that point,
// as we would've already made the split by then.
}
this.addToGlobalConflictCount(currentNode.GetConflict()); // TODO: Make CBS_GlobalConflicts use nodes that do this automatically after choosing a conflict
if (debug)
{
currentNode.Print();
}
if (currentNode.totalCost > currentCost) // Needs to be here because the goal may have a cost unseen before
{
currentCost = currentNode.totalCost;
this.nodesExpandedWithGoalCost = 0;
}
else if (currentNode.totalCost == currentCost) // check needed because macbs node cost isn't exactly monotonous
{
this.nodesExpandedWithGoalCost++;
}
// Check if node is the goal
if (currentNode.GoalTest())
{
//Debug.Assert(currentNode.totalCost >= maxExpandedNodeCostPlusH, "CBS goal node found with lower cost than the max cost node ever expanded: " + currentNode.totalCost + " < " + maxExpandedNodeCostPlusH);
// This is subtle, but MA-CBS may expand nodes in a non non-decreasing order:
// If a node with a non-optimal constraint is expanded and we decide to merge the agents,
// the resulting node can have a lower cost than before, since we ignore the non-optimal constraint
// because the conflict it addresses is between merged nodes.
// The resulting lower-cost node will have other constraints, that will raise the cost of its children back to at least its original cost,
// since the node with the non-optimal constraint was only expanded because its competitors that had an optimal
// constraint to deal with the same conflict apparently found the other conflict that I promise will be found,
// and so their cost was not smaller than this sub-optimal node.
// To make MA-CBS costs non-decreasing, we can choose not to ignore constraints that deal with conflicts between merged nodes.
// That way, the sub-optimal node will find a sub-optimal merged solution and get a high cost that will push it deep into the open list.
// But the cost would be to create a possibly sub-optimal merged solution where an optimal solution could be found instead, and faster,
// since constraints make the low-level heuristic perform worse.
// For an example for this subtle case happening, see problem instance 63 of the random grid with 4 agents,
// 55 grid cells and 9 obstacles.
if (debug)
{
Debug.WriteLine("-----------------");
}
this.totalCost = currentNode.totalCost;
this.solution = currentNode.CalculateJointPlan();
this.solutionDepth = this.totalCost - initialEstimate;
this.goalNode = currentNode; // Saves the single agent plans and costs
// The joint plan is calculated on demand.
this.Clear(); // Goal found - we're not going to resume this search
this.CleanGlobals();
return(true);
}
if (currentNode.totalCost >= this.targetCost || // Node is good enough
//(this.targetCost != int.MaxValue &&
//this.lowLevelGenerated > Math.Pow(Constants.NUM_ALLOWED_DIRECTIONS, this.instance.m_vAgents.Length))
this.solver.GetAccumulatedGenerated() > this.lowLevelGeneratedCap || // Stop because this is taking too long.
// We're looking at _generated_ low level nodes since that's an indication to the amount of work done,
// while expanded nodes is an indication of the amount of good work done.
// b**k is the maximum amount of nodes we'll generate if we expand this node with A*.
(this.milliCap != int.MaxValue && // (This check is much cheaper than the method call)
this.runner.ElapsedMilliseconds() > this.milliCap)) // Search is taking too long.
{
if (debug)
{
Debug.WriteLine("-----------------");
}
this.totalCost = maxExpandedNodeCostPlusH; // This is the min possible cost so far.
this.openList.Add(currentNode); // To be able to continue the search later
this.CleanGlobals();
return(false);
}
if (maxExpandedNodeCostPlusH < currentNode.totalCost + currentNode.h)
{
maxExpandedNodeCostPlusH = currentNode.totalCost + currentNode.h;
if (debug)
{
Debug.Print("New max F: {0}", maxExpandedNodeCostPlusH);
}
}
// Expand
bool wasUnexpandedNode = (currentNode.agentAExpansion == CbsNode.ExpansionState.NOT_EXPANDED &&
currentNode.agentBExpansion == CbsNode.ExpansionState.NOT_EXPANDED);
Expand(currentNode);
if (wasUnexpandedNode)
{
highLevelExpanded++;
}
// Consider moving the following into Expand()
if (currentNode.agentAExpansion == CbsNode.ExpansionState.EXPANDED &&
currentNode.agentBExpansion == CbsNode.ExpansionState.EXPANDED) // Fully expanded
{
currentNode.Clear();
}
}
this.totalCost = Constants.NO_SOLUTION_COST;
this.Clear(); // unsolvable problem - we're not going to resume it
this.CleanGlobals();
return(false);
}
/// <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);
}
protected bool MergeConflicting(CbsNode node)
{
return(node.ShouldMerge(mergeThreshold, globalConflictsCounter));
}
protected virtual bool ShouldMerge(CbsNode node)
{
return(node.ShouldMerge(mergeThreshold));
}
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);
}
public bool Solve()
{
this.SetGlobals(); // Again, because we might be resuming a search that was stopped.
//Debug.WriteLine("Solving Sub-problem On Level - " + mergeThreshold);
int initialEstimate = 0;
if (openList.Count > 0)
{
initialEstimate = ((CbsNode)openList.Peek()).totalCost;
}
int maxExpandedCost = -1;
while (openList.Count > 0)
{
// Check if max time has been exceeded
if (runner.ElapsedMilliseconds() > Constants.MAX_TIME)
{
totalCost = Constants.TIMEOUT_COST;
Console.WriteLine("Out of time");
this.Clear(); // Total search time exceeded - we're not going to resume this search.
this.CleanGlobals();
return(false);
}
var currentNode = (CbsNode)openList.Remove();
if (debug)
{
Debug.WriteLine("");
Debug.WriteLine("");
Debug.WriteLine("Node hash: " + currentNode.GetHashCode());
Debug.WriteLine("Total cost so far: " + currentNode.totalCost);
Debug.WriteLine("Expansion state: " + currentNode.agentAExpansion + ", " + currentNode.agentBExpansion);
Debug.WriteLine("Node depth: " + currentNode.depth);
var constraints = currentNode.GetConstraints();
Debug.WriteLine(constraints.Count.ToString() + " internal constraints so far: ");
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("Conflict: " + currentNode.GetConflict());
Debug.Write("Agent group assignments: ");
for (int i = 0; i < currentNode.agentsGroupAssignment.Length; i++)
{
Debug.Write(" " + currentNode.agentsGroupAssignment[i]);
}
Debug.WriteLine("");
Debug.Write("Single agent costs: ");
for (int i = 0; i < currentNode.allSingleAgentCosts.Length; i++)
{
Debug.Write(" " + currentNode.allSingleAgentCosts[i]);
}
Debug.WriteLine("");
currentNode.CalculateJointPlan().PrintPlan();
Debug.WriteLine("");
Debug.WriteLine("");
}
maxExpandedCost = Math.Max(maxExpandedCost, currentNode.totalCost);
// Check if node is the goal
if (currentNode.GoalTest())
{
Debug.Assert(currentNode.totalCost >= maxExpandedCost, "CBS goal node found with lower cost than the max cost node ever expanded: " + currentNode.totalCost + " < " + maxExpandedCost);
// This is subtle, but MA-CBS may expand nodes in a non non-decreasing order:
// If a node with a non-optimal constraint is expanded and we decide to merge the agents,
// the resulting node can have a lower cost than before, since we ignore the non-optimal constraint
// because the conflict it addresses is between merged nodes.
// The resulting lower-cost node will have other constraints, that will raise the cost of its children back to at least its original cost,
// since the node with the non-optimal constraint was only expanded because its competitors that had an optimal
// constraint to deal with the same conflict apparently found the other conflict that I promise will be found,
// and so their cost was not smaller than this sub-optimal node.
// To make MA-CBS costs non-decreasing, we can choose not to ignore constraints that deal with conflicts between merged nodes.
// That way, the sub-optimal node will find a sub-optimal merged solution and get a high cost that will push it deep into the open list.
// But the cost would be to create a possibly sub-optimal merged solution where an optimal solution could be found instead, and faster,
// since constraints make the low-level heuristic perform worse.
// For an example for this subtle case happening, see problem instance 63 of the random grid with 4 agents,
// 55 grid cells and 9 obstacles.
if (debug)
{
Debug.WriteLine("-------------------------");
}
this.totalCost = currentNode.totalCost;
this.solutionDepth = this.totalCost - initialEstimate;
this.goalNode = currentNode; // Saves the single agent plans and costs
// The joint plan is calculated on demand.
this.Clear(); // Goal found - we're not going to resume this search
this.CleanGlobals();
return(true);
}
if (currentNode.totalCost >= this.targetCost || // Node is good enough
//(this.targetCost != int.MaxValue &&
//this.lowLevelGenerated > Math.Pow(Constants.NUM_ALLOWED_DIRECTIONS, this.instance.m_vAgents.Length))
this.solver.GetAccumulatedGenerated() > this.lowLevelGeneratedCap || // Stop because this is taking too long.
// We're looking at _generated_ low level nodes since that's an indication to the amount of work done,
// while expanded nodes is an indication of the amount of good work done.
// b**k is the maximum amount of nodes we'll generate if we expand this node with A*.
(this.milliCap != int.MaxValue && // (This check is much cheaper than the method call)
this.runner.ElapsedMilliseconds() > this.milliCap)) // Search is taking too long.
{
if (debug)
{
Debug.WriteLine("-------------------------");
}
this.totalCost = maxExpandedCost; // This is the min possible cost so far.
this.openList.Add(currentNode); // To be able to continue the search later
this.CleanGlobals();
return(false);
}
// Expand
bool wasUnexpandedNode = (currentNode.agentAExpansion == CbsNode.ExpansionState.NOT_EXPANDED &&
currentNode.agentBExpansion == CbsNode.ExpansionState.NOT_EXPANDED);
Expand(currentNode);
if (wasUnexpandedNode)
{
highLevelExpanded++;
}
// Consider moving the following into Expand()
if (currentNode.agentAExpansion == CbsNode.ExpansionState.EXPANDED &&
currentNode.agentBExpansion == CbsNode.ExpansionState.EXPANDED) // Fully expanded
{
currentNode.Clear();
}
}
this.totalCost = Constants.NO_SOLUTION_COST;
this.Clear(); // unsolvable problem - we're not going to resume it
this.CleanGlobals();
return(false);
}
