/// <summary>
/// Runs the algorithm until the problem is solved or memory/time is exhausted
/// </summary>
/// <returns>True if solved</returns>
public virtual bool Solve()
{
int initialEstimate = ((WorldState)openList.Peek()).h;
int lastF = -1;
WorldState lastNode = null;
bool debug = false;
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();
return(false);
}
var currentNode = (WorldState)openList.Remove();
Debug.Assert(currentNode.g + currentNode.h >= lastF,
"A* node with decreasing F: " + (currentNode.g + currentNode.h) + " < " + lastF + ".");
// FIXME: Better to use the whole rich comparison. Save the last node and use compareTo.
// The only time a node is allowed to be smaller than the last one is if it's the goal.
// Is that even true? A child is smaller than its parent if it's on the heuristic path to the goal because its g i larger.
lastF = currentNode.g + currentNode.h;
lastNode = currentNode;
// Calculate expansion delay
int expansionDelay = this.expanded - currentNode.expandedCountWhenGenerated - 1; // -1 to make the delay zero when a node is expanded immediately after being generated.
maxExpansionDelay = Math.Max(maxExpansionDelay, expansionDelay);
// Check if node is the goal, or knows how to get to it
if (currentNode.GoalTest())
{
this.totalCost = currentNode.GetGoalCost();
this.singleCosts = currentNode.GetSingleCosts();
this.solution = currentNode.GetPlan();
this.singlePlans = currentNode.GetSinglePlans();
this.solutionDepth = this.totalCost - initialEstimate;
this.Clear();
return(true);
}
// Expand
Expand(currentNode);
expanded++;
}
totalCost = Constants.NO_SOLUTION_COST;
this.Clear();
return(false);
}
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>
/// Runs the algorithm until the problem is solved or memory/time is exhausted
/// </summary>
/// <returns>True if solved</returns>
public virtual bool Solve()
{
int initialEstimate = ((WorldState)openList.Peek()).h; // g=0 initially
int lastF = -1;
WorldState lastNode = null;
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.solutionDepth = ((WorldState)openList.Peek()).f - initialEstimate; // A minimum estimate
this.Clear();
return(false);
}
var currentNode = (WorldState)openList.Remove();
if (debug)
{
Debug.WriteLine("");
Debug.WriteLine("Expanding node: " + currentNode);
}
lastF = currentNode.f;
lastNode = currentNode;
// Calculate expansion delay
int expansionDelay = this.expanded - currentNode.expandedCountWhenGenerated - 1; // -1 to make the delay zero when a node is expanded immediately after being generated.
maxExpansionDelay = Math.Max(maxExpansionDelay, expansionDelay);
// Check if node is the goal, or knows how to get to it
if (currentNode.GoalTest())
{
//((IReadOnlyDictionary<TimedMove, List<int>>)instance.parameters[CBS_LocalConflicts.CAT]), conflictRange);
/*List<TimedMove> newList = new List<TimedMove>();
* WorldState current = currentNode;
* while(current != null)
* {
* newList.Add(current.)
* }*/
this.conflictTimes = currentNode.conflictTimes;
this.totalCost = currentNode.GetGoalCost();
this.singleCosts = currentNode.GetSingleCosts();
this.solution = currentNode.GetPlan();
this.singlePlans = currentNode.GetSinglePlans();
this.conflictCounts = currentNode.cbsInternalConflicts; // TODO: Technically, could be IndependenceDetection's count. Merge them.
this.conflictTimes = currentNode.conflictTimes;
this.solutionDepth = this.totalCost - initialEstimate;
this.Clear();
return(true);
}
Expand(currentNode);
expanded++;
}
totalCost = Constants.NO_SOLUTION_COST;
this.Clear();
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);
}
