/// <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, MAM_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;
CFMCbsNode root = new CFMCbsNode(instance.m_vAgents.Length, this); // Problem instance and various strategy data is all passed under 'this'.
// Solve the root node - Solve with MMStar, and find conflicts
bool solved = root.Solve();
if (solved && root.totalCost <= this.maxCost)
{
this.openList.Add(root);
this.highLevelGenerated++;
this.closedList.Add(root, root);
}
}
public HashSet <CFMCbsConstraint> GetConstraints()
{
var constraints = new HashSet <CFMCbsConstraint>();
CFMCbsNode current = this;
CFMCbsConstraint currentConstraint = null;
while (current.depth > 0) // The root has no constraints
{
if (current.constraint != null && // Next check not enough if "surprise merges" happen (merges taken from adopted child)
current.prev.conflict != null && // Can only happen for temporary lookahead nodes the were created and then later the parent adopted a goal node
this.agentsGroupAssignment[current.prev.conflict.agentAIndex] !=
this.agentsGroupAssignment[current.prev.conflict.agentBIndex]) // 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.
// Also, merging creates a non-root node with a null constraint, and this helps avoid adding the null to the answer.
{
currentConstraint = current.constraint;
}
TimedMove currentMove = current.constraint.move;
CFMCbsConstraint newConstraint = new CFMCbsConstraint(currentConstraint.agentNum, currentMove.x, currentMove.y, currentMove.direction, currentMove.time);
constraints.Add(newConstraint);
current = current.prev;
}
return(constraints);
}
public CFMCbsNode(int numberOfAgents, CFM_CBS cbs, ushort[] agentsGroupAssignment = null)
{
this.cbs = cbs;
mamPlan = null;
mamCost = -1;
allSingleAgentCosts = new int[numberOfAgents];
countsOfInternalAgentsThatConflict = new int[numberOfAgents];
this.nodeConflicts = null;
if (agentsGroupAssignment == null)
{
this.agentsGroupAssignment = new ushort[numberOfAgents];
for (ushort i = 0; i < numberOfAgents; i++)
{
this.agentsGroupAssignment[i] = i;
}
}
else
{
this.agentsGroupAssignment = agentsGroupAssignment.ToArray <ushort>();
}
agentNumToIndex = new Dictionary <int, int>();
for (int i = 0; i < numberOfAgents; i++)
{
agentNumToIndex[this.cbs.GetProblemInstance().m_vAgents[i].agentIndex] = i;
}
depth = 0;
replanSize = 1;
agentAExpansion = ExpansionState.NOT_EXPANDED;
agentBExpansion = ExpansionState.NOT_EXPANDED;
this.prev = null;
this.constraint = null;
this.solver = solver;
this.singleAgentSolver = singleAgentSolver;
}
public int CompareTo(IBinaryHeapItem item)
{
CFMCbsNode other = (CFMCbsNode)item;
return((int)(this.mamCost - other.mamCost));
}
private bool isLeftNode(CFMCbsNode node)
{
if (node.prev == null || node.agentToReplan == node.prev.conflict.agentAIndex)
{
return(true);
}
return(false);
}
public int CompareToIgnoreH(CFMCbsNode other, bool ignorePartialExpansion = false)
{
// Tie breaking:
// Prefer larger cost - higher h usually means more work needs to be done
if (this.totalCost > other.totalCost)
{
return(-1);
}
if (this.totalCost < other.totalCost)
{
return(1);
}
// Prefer less external conflicts, even over goal nodes, as goal nodes with less external conflicts are better.
// External conflicts are also taken into account by the low level solver to prefer less conflicts between fewer agents.
// This only helps when this CBS is used as a low level solver, of course.
if (this.totalConflictsWithExternalAgents < other.totalConflictsWithExternalAgents)
{
return(-1);
}
if (this.totalConflictsWithExternalAgents > other.totalConflictsWithExternalAgents)
{
return(1);
}
if (this.totalExternalAgentsThatConflict < other.totalExternalAgentsThatConflict)
{
return(-1);
}
if (this.totalExternalAgentsThatConflict > other.totalExternalAgentsThatConflict)
{
return(1);
}
// 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);
}
if (this.minOpsToSolve < other.minOpsToSolve)
{
return(-1);
}
if (this.minOpsToSolve > other.minOpsToSolve)
{
return(1);
}
return(0);
}
public virtual void Expand(CFMCbsNode node)
{
ushort parentCost = node.totalCost;
ushort parentH = node.h;
IList <CFMCbsNode> children = null; // To quiet the compiler
bool reinsertParent = false; // To quiet the compiler
this.ExpandImpl(node, out children, out reinsertParent);
foreach (var child in children)
{
closedList.Add(child, child);
this.highLevelGenerated++;
openList.Add(child);
}
}
/// <summary>
/// Child from merge action constructor. FIXME: Code dup with previous constructor.
/// </summary>
/// <param name="father"></param>
/// <param name="mergeGroupA"></param>
/// <param name="mergeGroupB"></param>
public CFMCbsNode(CFMCbsNode father, int mergeGroupA, int mergeGroupB)
{
mamPlan = null;
mamCost = -1;
this.allSingleAgentCosts = father.allSingleAgentCosts.ToArray <int>();
this.countsOfInternalAgentsThatConflict = father.countsOfInternalAgentsThatConflict.ToArray <int>();
this.nodeConflicts = null;
this.agentsGroupAssignment = father.agentsGroupAssignment.ToArray <ushort>();
this.agentNumToIndex = father.agentNumToIndex;
this.prev = father;
this.constraint = null;
this.depth = (ushort)(this.prev.depth + 1);
this.agentAExpansion = ExpansionState.NOT_EXPANDED;
this.agentBExpansion = ExpansionState.NOT_EXPANDED;
this.replanSize = 1;
this.solver = father.solver;
this.singleAgentSolver = father.singleAgentSolver;
this.cbs = father.cbs;
}
/// <summary>
/// Checks the group assignment and the constraints
/// </summary>
/// <param name="obj"></param>
/// <returns></returns>
public override bool Equals(object obj)
{
CFMCbsNode other = (CFMCbsNode)obj;
if (this.agentsGroupAssignment.SequenceEqual <ushort>(other.agentsGroupAssignment) == false)
{
return(false);
}
CFMCbsNode current = this;
HashSet <CFMCbsConstraint> other_constraints = other.GetConstraints();
HashSet <CFMCbsConstraint> constraints = this.GetConstraints();
foreach (CFMCbsConstraint constraint in constraints)
{
if (other_constraints.Contains(constraint) == false)
{
return(false);
}
//current = current.prev; dor comment
}
return(constraints.Count == other_constraints.Count);
}
/// <summary>
/// Create Constraints from conflict
/// </summary>
/// <param name="node"></param>
/// <param name="doLeftChild"></param>
/// <param name="closedListHitChildCost"></param>
/// <returns></returns>
protected CFMCbsNode ConstraintExpand(CFMCbsNode node, bool doLeftChild, out int closedListHitChildCost)
{
CFMCbsConflict conflict = node.GetConflict();
int conflictingAgentIndex = doLeftChild? conflict.agentAIndex : conflict.agentBIndex;
CFMCbsNode.ExpansionState expansionsState = doLeftChild ? node.agentAExpansion : node.agentBExpansion;
CFMCbsNode.ExpansionState otherChildExpansionsState = doLeftChild ? node.agentBExpansion : node.agentAExpansion;
string agentSide = doLeftChild? "left" : "right";
int groupSize = node.GetGroupSize(conflictingAgentIndex);
closedListHitChildCost = -1;
if (expansionsState != CFMCbsNode.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 = CFMCbsNode.ExpansionState.EXPANDED;
}
else
{
node.agentBExpansion = CFMCbsNode.ExpansionState.EXPANDED;
}
var newConstraint = new CFMCbsConstraint(conflict, instance, doLeftChild);
CFMCbsNode child = new CFMCbsNode(node, newConstraint, conflictingAgentIndex);
if (closedList.ContainsKey(child) == false)
{
bool success = child.Solve();
if (success == false)
{
return(null); // A timeout probably occured
}
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(CFMCbsNode node, out IList <CFMCbsNode> children, out bool reinsertParent)
{
CFMCbsConflict conflict = node.GetConflict();
children = new List <CFMCbsNode>();
CFMCbsNode 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 bool Solve()
{
//this.SetGlobals(); // Again, because we might be resuming a search that was stopped.
int initialEstimate = 0;
if (openList.Count > 0)
{
initialEstimate = ((CFMCbsNode)openList.Peek()).totalCost;
}
int currentCost = -1;
Console.WriteLine("maxTime: " + Constants.MAX_TIME);
this.startTime = this.ElapsedMillisecondsTotal();
while (openList.Count > 0)
{
//Console.WriteLine(this.ElapsedMilliseconds() / 10);
//Console.WriteLine(openList.Count);
//Console.WriteLine(closedList.Count);
// Check if max time has been exceeded
if (this.ElapsedMilliseconds() > Constants.MAX_TIME)
{
this.totalCost = Constants.TIMEOUT_COST;
Console.WriteLine("Out of time");
this.solutionDepth = ((CFMCbsNode)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 = (CFMCbsNode)openList.Remove();
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 = (int)currentNode.mamCost;
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();
this.solved = true;
return(true);
}
currentNode.ChooseConflict();
// Expand
bool wasUnexpandedNode = (currentNode.agentAExpansion == CFMCbsNode.ExpansionState.NOT_EXPANDED &&
currentNode.agentBExpansion == CFMCbsNode.ExpansionState.NOT_EXPANDED);
Expand(currentNode);
if (wasUnexpandedNode)
{
highLevelExpanded++;
}
// Consider moving the following into Expand()
if (currentNode.agentAExpansion == CFMCbsNode.ExpansionState.EXPANDED &&
currentNode.agentBExpansion == CFMCbsNode.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);
}
