/// <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 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;
}
/// <summary>
/// Solves the entire node - finds a plan for every agent group.
/// Since this method is only called for the root of the constraint tree, every agent is in its own group.
/// </summary>
/// <param name="depthToReplan"></param>
/// <returns></returns>
public bool Solve()
{
this.totalCost = 0;
ProblemInstance problem = this.cbs.GetProblemInstance();
HashSet <CFMCbsConstraint> newConstraints = this.GetConstraints(); // Probably empty as this is probably the root of the CT.
// Constraints initiated with the problem instance
//var constraints = (HashSet_U<CbsConstraint>)problem.parameters[MAPF_CBS.CONSTRAINTS];
var constraints = new HashSet_U <CFMCbsConstraint>();
Dictionary <int, int> agentsWithConstraints = null;
if (constraints.Count != 0)
{
int maxConstraintTimeStep = constraints.Max <CFMCbsConstraint>(constraint => constraint.time);
agentsWithConstraints = constraints.Select <CFMCbsConstraint, int>(constraint => constraint.agentNum).Distinct().ToDictionary <int, int>(x => x); // ToDictionary because there's no ToSet...
}
constraints.Join(newConstraints);
// This mechanism of adding the constraints to the possibly pre-existing constraints allows having
// layers of CBS solvers, each one adding its own constraints and respecting those of the solvers above it.
// Solve using MMMStar
HashSet <MMStarConstraint> mConstraints = importCBSConstraintsToMMStarConstraints(constraints);
this.cbs.runner.SolveGivenProblem(problem, mConstraints);
this.mamPlan = this.cbs.runner.plan;
this.mamCost = this.cbs.runner.solutionCost;
// Gather conflicts
this.nodeConflicts = gatherConflicts();
//if(MAM_Run.toPrint)
// printConflicts(allSingleAgentPlans);
this.isGoal = this.nodeConflicts.Count == 0;
return(true);
}
/// <summary>
/// Write to file a given instance
/// </summary>
/// <param name="instance">The instance to execute</param>
public void WriteGivenProblem
(
ProblemInstance instance,
MAM_ISolver solver,
MAM_Plan currentPlan = null)
{
string initialH;
if (solver.GetHeuristicCalculator().GetInitialH() == 0)
{
initialH = 0.ToString();
}
else if (solver.GetCostFunction() == CostFunction.SOC)
{
Tuple <double, int> bestInitH = solver.GetHeuristicCalculatorInitialH();
double bestH = bestInitH.Item1;
double bestAgents = bestInitH.Item2;
initialH = bestH.ToString();
}
else
{
Tuple <double, int> bestInitH = solver.GetHeuristicCalculatorInitialH();
double bestH = bestInitH.Item1;
int bestAgents = bestInitH.Item2;
initialH = (bestH / bestAgents).ToString();
}
writeToFile(
solver.GetName(), // solver name
planningTime.ToString(), // planning time
costFunctionToString(solver.GetCostFunction()), // cost function
solver.GetSolutionSOCCost().ToString(), // solution SOC cost
solver.GetSolutionMakeSpanCost().ToString(), // solution MakeSpan cost
instanceId.ToString(), // instanceId
instance.fileName, // file Name
instance.m_vAgents.Length.ToString(), // #Agents
m_mapFileName, // Map name
solver.IsSolved().ToString(), // Success
instance.m_nObstacles, // Obstacles
solver.GetExpanded().ToString(), // Expansions
solver.GetGenerated().ToString(), // Generates
preprocessingTime.ToString(), // preprocessing time
solver.GetHeuristicCalculator().GetName(), // Heuristic Name
initialH); // Initial h value
}
/// <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,
HashSet <MMStarConstraint> constraints = null
)
{
this.instance = problemInstance;
this.runner = runner;
this.ClearPrivateStatistics();
this.totalCost = 0;
this.solutionDepth = -1;
this.milliCap = int.MaxValue;
this.goalLocation = null;
this.solution = null;
this.bestMakeSpanCost = int.MaxValue;
this.bestSOCCost = int.MaxValue;
this.bestCostLocation = null;
this.meetFlag = false;
this.success = false;
this.openList = new MAM_OpenList(this);
if (constraints != null)
{
this.constraints = constraints;
}
else
{
this.constraints = new HashSet <MMStarConstraint>();
}
AddSubSetHeuristics();
foreach (MAM_AgentState agent in this.instance.m_vAgents)
{
agent.numOfAgentsInBestHeuristic = this.instance.m_vAgents.Length;
CalculateH(agent, null);
CalculateF(agent);
closed(agent);
openList.Add(agent);
}
}
public virtual MAM_Plan GetPlan()
{
if (this.solution == null)
{
if (bestCostLocation == null)
{
return(null);
}
Dictionary <int, Dictionary <int, MAM_AgentState> > solutionAgentsStatesDictionaries = closedList[bestCostLocation];
Dictionary <int, MAM_AgentState> solutionAgentsStates = new Dictionary <int, MAM_AgentState>();
foreach (int agent in solutionAgentsStatesDictionaries.Keys)
{
int bestTimeForGivenAgent = solutionAgentsStatesDictionaries[agent].Keys.Min();
MAM_AgentState bestStateForGivenAgent = solutionAgentsStatesDictionaries[agent][bestTimeForGivenAgent];
solutionAgentsStates.Add(agent, bestStateForGivenAgent);
}
this.solution = new MAM_Plan(solutionAgentsStates.Values.ToList());
}
return(this.solution);
}
/// <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;
}
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);
}
/// <summary>
/// Construct with chosen algorithms.
/// </summary>
public MAM_Run()
{
this.watch = Stopwatch.StartNew();
// Preparing the solvers:
solvers = new List <MAM_ISolver>();
// FastMap Heuristic
/*
* //ISolver
* MAM_ISolver MMStar_FastMapH_Makespan = new MM_Star(MM_Star.CostFunction.MakeSpan);
* MAM_ISolver MMStar_FastMapH_SOC = new MM_Star(MM_Star.CostFunction.SOC);
* MAM_HeuristicCalculator FastMapHCalculator = new FastMapHCalculator();
* //MMStar_FastMapH_Makespan.SetHeuristic(FastMapHCalculator);
* MMStar_FastMapH_SOC.SetHeuristic(FastMapHCalculator);
*/
// Median Heuristic
//ISolver
MAM_ISolver MMStar_MedianH_Makespan = new MM_Star(MM_Star.CostFunction.MakeSpan);
MAM_ISolver MMStar_MedianH_SOC = new MM_Star(MM_Star.CostFunction.SOC);
MAM_HeuristicCalculator MedianHCalculator = new MedianHCalculator();
MMStar_MedianH_Makespan.SetHeuristic(MedianHCalculator);
//MMStar_MedianH_SOC.SetHeuristic(MedianHCalculator);
// Clique Heuristic
//ISolver
MAM_ISolver MMStar_CliqueH_Makespan = new MM_Star(MM_Star.CostFunction.MakeSpan);
MAM_ISolver MMStar_CliqueH_SOC = new MM_Star(MM_Star.CostFunction.SOC);
MAM_HeuristicCalculator CliqueHeuristic = new CliqueHCalculator();
MMStar_CliqueH_Makespan.SetHeuristic(CliqueHeuristic);
//MMStar_CliqueH_SOC.SetHeuristic(CliqueHeuristic);
// No Heuristic
//ISolver
MAM_ISolver MMStar_ZeroH_Makespan = new MM_Star(MM_Star.CostFunction.MakeSpan);
MAM_ISolver MMStar_ZeroeH_SOC = new MM_Star(MM_Star.CostFunction.SOC);
MAM_HeuristicCalculator ZeroHeuristic = new ZeroHCalculator();
MMStar_ZeroH_Makespan.SetHeuristic(ZeroHeuristic);
//MMStar_ZeroeH_SOC.SetHeuristic(ZeroHeuristic);
//MAM_ISolver MMStar_LPH_Makespan = new MM_Star(MM_Star.CostFunction.MakeSpan);
//MAM_HeuristicCalculator LPHCalculator = new LPHCalculator();
//MMStar_LPH_Makespan.SetHeuristic(LPHCalculator);
// ***** Makespan Solvers *****
//solvers.Add(MMStar_FastMapH_Makespan);
//solvers.Add(MMStar_MedianH_Makespan);
solvers.Add(MMStar_CliqueH_Makespan);
//solvers.Add(MMStar_ZeroH_Makespan);
// ***** SOC Solvers *****
//solvers.Add(MMStar_FastMapH_SOC);
//solvers.Add(MMStar_MedianH_SOC);
//solvers.Add(MMStar_CliqueH_SOC);
//solvers.Add(MMStar_ZeroeH_SOC);
outOfTimeCounters = new int[solvers.Count];
for (int i = 0; i < outOfTimeCounters.Length; i++)
{
outOfTimeCounters[i] = 0;
}
this.plan = null;
}
/// <summary>
/// Solve given instance with a list of algorithms
/// </summary>
/// <param name="instance">The instance to solve</param>
public bool SolveGivenProblem
(
ProblemInstance instance,
HashSet <MMStarConstraint> constraints = null
)
{
instanceId += 1;
bool success = true;
List <uint> agentList = Enumerable.Range(0, instance.m_vAgents.Length).Select <int, uint>(x => (uint)x).ToList <uint>(); // FIXME: Must the heuristics really receive a list of uints?
// Solve using the different algorithms
//Debug.WriteLine("Solving " + instance);
MAM_AgentState[] vAgents = new MAM_AgentState[instance.GetNumOfAgents()];
for (int agentIndex = 0; agentIndex < instance.GetNumOfAgents(); agentIndex++)
{
vAgents[agentIndex] = new MAM_AgentState(instance.m_vAgents[agentIndex]);
}
for (int i = 0; i < solvers.Count; i++)
{
solutionCost = -1;
if (outOfTimeCounters[i] < Constants.MAX_FAIL_COUNT)
{
GC.Collect();
GC.WaitForPendingFinalizers();
preprocessingTime = 0;
if (solvers[i].GetHeuristicCalculator().GetName() == "FastMap H")
{
this.startTime = this.ElapsedMillisecondsTotal();
solvers[i].GetHeuristicCalculator().init(instance);
solvers[i].GetHeuristicCalculator().preprocessing();
preprocessingTime = this.ElapsedMilliseconds();
Console.WriteLine("Preprocessing time in milliseconds: {0}", preprocessingTime);
}
this.run(solvers[i], instance, constraints);
MAM_AgentState[] vAgents2 = new MAM_AgentState[vAgents.Count()];
for (int agentIndex = 0; agentIndex < vAgents.Count(); agentIndex++)
{
vAgents2[agentIndex] = new MAM_AgentState(vAgents[agentIndex]);
}
instance.m_vAgents = vAgents2;
if (solvers[i].GetCostFunction() == CostFunction.MakeSpan)
{
solutionCost = solvers[i].GetSolutionMakeSpanCost();
}
else if (solvers[i].GetCostFunction() == CostFunction.SOC)
{
solutionCost = solvers[i].GetSolutionSOCCost();
}
MAM_Plan plan = null;
if (solvers[i].IsSolved()) // Solved successfully
{
plan = solvers[i].GetPlan();
if (toPrint)
{
Console.WriteLine();
plan.ToString();
Console.WriteLine();
}
outOfTimeCounters[i] = 0;
//Console.WriteLine("+SUCCESS+ (:");
this.plan = plan;
}
else
{
outOfTimeCounters[i]++;
Console.WriteLine("-FAILURE- ):");
}
planningTime = elapsedTime;
}
else if (toPrint)
{
PrintNullStatistics(solvers[i]);
}
}
return(true);
}
/// <summary>
/// Worth doing because the node may always be in the closed list
/// </summary>
public void Clear()
{
this.mamPlan = null;
this.allSingleAgentCosts = null;
}
