public CBS(ICbsSolver singleAgentSolver, ICbsSolver generalSolver)
{
this.closedList = new Dictionary <CbsNode, CbsNode>();
this.openList = new OpenList(this);
this.solver = generalSolver;
this.singleAgentSolver = singleAgentSolver;
}
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>
/// Print the solver statistics to the results file.
/// </summary>
/// <param name="instance">The problem instance that was solved. Not used!</param>
/// <param name="CFMAMSolver">The solver that solved the problem instance</param>
/// <param name="runtimeInMillis">The time it took the given solver to solve the given instance</param>
private void PrintStatistics
(
ProblemInstance instance,
double runtimeInMillis,
IMS_ISolver CFMAMSolver,
ICbsSolver CBSMMStarSolver = null
)
{
// Success col:
//if (solver.GetSolutionCost() < 0)
// this.resultsWriter.Write(Run.FAILURE_CODE + RESULTS_DELIMITER);
//else
this.resultsWriter.Write(MAM_Run.SUCCESS_CODE + RESULTS_DELIMITER);
// Runtime col:
this.resultsWriter.Write(runtimeInMillis + RESULTS_DELIMITER);
// Solution Cost col:
//this.resultsWriter.Write(solver.GetSolutionCost() + RESULTS_DELIMITER);
if (CBSMMStarSolver != null)
{
CBSMMStarSolver.OutputStatistics(this.resultsWriter);
this.resultsWriter.Write(CBSMMStarSolver.GetSolutionDepth() + RESULTS_DELIMITER);
}
else
{
// Algorithm specific cols:
CFMAMSolver.OutputStatistics(this.resultsWriter);
// Solution Depth col:
this.resultsWriter.Write(CFMAMSolver.GetSolutionDepth() + RESULTS_DELIMITER);
}
//this.resultsWriter.Flush();
}
private void runCFMCBS(ICbsSolver solver, ProblemInstance instance)
{
// Run the algorithm
bool solved;
Console.WriteLine("----------------- " + solver + ", Minimizing MakeSpan -----------------");
Constants.ALLOW_WAIT_MOVE = true;
this.startTime = this.ElapsedMillisecondsTotal();
solver.Setup(instance, new MAM_Run()); // Defining MMStar as the low level of the algorithm
solved = solver.Solve();
elapsedTime = this.ElapsedMilliseconds();
if (solved)
{
Console.WriteLine("Total MakeSpan cost: {0}", solver.GetSolutionCost());
}
else
{
Console.WriteLine("Failed to solve CBS");
}
Console.WriteLine();
Console.WriteLine("Expanded nodes: {0}", solver.GetExpanded());
Console.WriteLine("Time in milliseconds: {0}", elapsedTime);
if (toPrint)
{
this.PrintStatistics(instance, elapsedTime, null, solver);
}
}
public CBS_LocalConflicts(ICbsSolver singleAgentSolver, ICbsSolver generalSolver, int mergeThreshold = -1)
{
this.closedList = new Dictionary <CbsNode, CbsNode>();
this.openList = new OpenList(this);
this.mergeThreshold = mergeThreshold;
this.solver = generalSolver;
this.singleAgentSolver = singleAgentSolver;
}
public CFMCBS_OpenList(ICbsSolver user)
{
this.heap = new BinaryHeap();
this.queue = new Queue <IBinaryHeapItem>();
this.user = user;
this.ClearPrivateStatistics();
this.ClearPrivateAccumulatedStatistics();
}
public CBS_IDA(ICbsSolver solver, int maxThreshold = -1, int currentThreshold = -1)
{
this.mergeThreshold = currentThreshold;
this.solver = solver;
this.lowLevelSolver = solver;
this.maxThreshold = maxThreshold;
if (currentThreshold < maxThreshold)
{
//this.solver = new CBS_GlobalConflicts(solver, maxThreshold, currentThreshold + 1);
}
}
public IterativeDeepeningCBS(ICbsSolver singleAgentSolver, ICbsSolver generalSolver, int maxThreshold = -1,
int currentThreshold = -1, IHeuristicCalculator <CbsNode> heuristic = null)
{
this.mergeThreshold = currentThreshold;
this.solver = generalSolver;
this.singleAgentSolver = singleAgentSolver;
this.maxThreshold = maxThreshold;
this.heuristic = heuristic;
if (currentThreshold < maxThreshold)
{
//this.solver = new MACBS_WholeTreeThreshold(solver, maxThreshold, currentThreshold + 1);
}
}
/// <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;
}
private void WriteGivenCFMCBSProblem(ProblemInstance instance, ICbsSolver solver, string plan)
{
writeToFile(
solver.GetName(), // solver name
planningTime.ToString(), // planning time
"MakeSpan", // cost function
solver.GetSolutionCost().ToString(), // solution 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
}
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;
}
/// <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>
/// For CBS IDA* only.
/// Consider inheriting from CbsNode and overriding the Replan method instead.
/// </summary>
/// <param name="agentForReplan"></param>
/// <param name="depthToReplan"></param>
/// <returns></returns>
public bool Replan3b(int agentForReplan, int depthToReplan)
{
var newInternalCAT = new Dictionary <TimedMove, List <int> >();
HashSet <CbsConstraint> newConstraints = this.GetConstraints();
var InternalCAT = (Dictionary_U <TimedMove, int>)problem.parameters[CBS_LocalConflicts.INTERNAL_CAT];
var Constraints = (HashSet_U <CbsConstraint>)problem.parameters[CBS_LocalConflicts.CONSTRAINTS];
List <CbsConstraint> mustConstraints = this.GetMustConstraints();
problem.parameters[CBS_LocalConflicts.MUST_CONSTRAINTS] = mustConstraints;
if (newConstraints.Count != 0)
{
int maxConstraintTimeStep = newConstraints.Max <CbsConstraint>(constraint => constraint.time);
depthToReplan = Math.Max(depthToReplan, maxConstraintTimeStep); // Give all constraints a chance to affect the plan
}
//Debug.WriteLine("Sub-problem:");
List <AgentState> subGroup = new List <AgentState>();
int groupNum = this.agentsGroupAssignment[agentForReplan];
for (int i = 0; i < agentsGroupAssignment.Length; i++)
{
if (this.agentsGroupAssignment[i] == groupNum)
{
subGroup.Add(problem.m_vAgents[i]);
// Debug.WriteLine(i);
}
else
{
allSingleAgentPlans[i].AddPlanToCAT(newInternalCAT, totalCost);
}
}
this.replanSize = (ushort)subGroup.Count;
ICbsSolver relevantSolver = this.solver;
if (subGroup.Count == 1)
{
relevantSolver = this.singleAgentSolver;
}
ProblemInstance subProblem = problem.Subproblem(subGroup.ToArray());
subProblem.parameters = problem.parameters;
InternalCAT.Join(newInternalCAT);
Constraints.Join(newConstraints);
//constraints.Print();
relevantSolver.Setup(subProblem, depthToReplan, runner);
bool solved = relevantSolver.Solve();
relevantSolver.AccumulateStatistics();
relevantSolver.ClearStatistics();
if (solved == false)
{
InternalCAT.Seperate(newInternalCAT);
Constraints.Seperate(newConstraints);
return(false);
}
int j = 0;
SinglePlan[] singlePlans = relevantSolver.GetSinglePlans();
int[] singleCosts = relevantSolver.GetSingleCosts();
for (int i = 0; i < agentsGroupAssignment.Length; i++)
{
if (this.agentsGroupAssignment[i] == groupNum)
{
this.allSingleAgentPlans[i] = singlePlans[j];
this.allSingleAgentCosts[i] = singleCosts[j];
j++;
}
}
Debug.Assert(j == replanSize);
// Calc totalCost
this.totalCost = (ushort)this.allSingleAgentCosts.Sum();
// PrintPlan();
InternalCAT.Seperate(newInternalCAT);
Constraints.Seperate(newConstraints);
newConstraints.Clear();
this.FindConflict();
// PrintConflict();
return(true);
}
/// <summary>
/// Replan for a given agent (when constraints for that agent have changed).
/// FIXME: Code duplication with Solve().
/// </summary>
/// <param name="agentForReplan"></param>
/// <param name="depthToReplan">CBS's minDepth param</param>
/// <param name="newInternalCAT"></param>
/// <returns></returns>
public bool Replan(int agentForReplan, int depthToReplan, Dictionary <TimedMove, List <int> > newInternalCAT = null, List <AgentState> subGroup = null)
{
int groupNum = this.agentsGroupAssignment[agentForReplan];
if (newInternalCAT == null && subGroup == null)
{
newInternalCAT = new Dictionary <TimedMove, List <int> >();
subGroup = new List <AgentState>();
int maxPlanSize = this.allSingleAgentPlans.Max <SinglePlan>(plan => plan.GetSize());
for (int i = 0; i < agentsGroupAssignment.Length; i++)
{
if (this.agentsGroupAssignment[i] == groupNum)
{
subGroup.Add(problem.m_vAgents[i]);
}
else
{
allSingleAgentPlans[i].AddPlanToCAT(newInternalCAT, maxPlanSize);
}
}
}
HashSet <CbsConstraint> newConstraints = this.GetConstraints();
var internalCAT = (Dictionary_U <TimedMove, int>)problem.parameters[CBS_LocalConflicts.INTERNAL_CAT];
var constraints = (HashSet_U <CbsConstraint>)problem.parameters[CBS_LocalConflicts.CONSTRAINTS];
// Construct the subgroup of agents that are of the same group as agentForReplan,
// and add the plans of all other agents to newInternalCAT
this.replanSize = (ushort)subGroup.Count;
ICbsSolver relevantSolver = this.solver;
if (subGroup.Count == 1)
{
relevantSolver = this.singleAgentSolver;
}
ProblemInstance subProblem = problem.Subproblem(subGroup.ToArray());
internalCAT.Join(newInternalCAT);
constraints.Join(newConstraints);
if (constraints.Count != 0)
{
int maxConstraintTimeStep = constraints.Max <CbsConstraint>(constraint => constraint.time);
depthToReplan = Math.Max(depthToReplan, maxConstraintTimeStep); // Give all constraints a chance to affect the plan
}
relevantSolver.Setup(subProblem, depthToReplan, runner);
bool solved = relevantSolver.Solve();
relevantSolver.AccumulateStatistics();
relevantSolver.ClearStatistics();
internalCAT.Seperate(newInternalCAT);
constraints.Seperate(newConstraints);
if (solved == false) // Usually means a timeout occured.
{
return(false);
}
// Copy the SinglePlans for the solved agent group from the solver to the appropriate places in this.allSingleAgentPlans
int j = 0;
SinglePlan[] singlePlans = relevantSolver.GetSinglePlans();
int[] singleCosts = relevantSolver.GetSingleCosts();
for (int i = 0; i < agentsGroupAssignment.Length; i++)
{
if (this.agentsGroupAssignment[i] == groupNum)
{
this.allSingleAgentPlans[i] = singlePlans[j];
this.allSingleAgentCosts[i] = singleCosts[j];
j++;
}
}
Debug.Assert(j == replanSize);
// Calc totalCost
this.totalCost = (ushort)this.allSingleAgentCosts.Sum();
// PrintPlan();
this.FindConflict();
// PrintConflict();
return(true);
}
public CBS_GlobalConflicts(ICbsSolver singleAgentSolver, ICbsSolver generalSolver, int mergeThreshold = -1)
: base(singleAgentSolver, generalSolver, mergeThreshold)
{
}
