} // Nonsense values until Init, just allocate move
public CbsConstraint(CbsConflict conflict, ProblemInstance instance, bool agentA, Run.ConstraintPolicy constraintPolicy = Run.ConstraintPolicy.Single, int constraintRange = 0)
{
Move move;
int agentNum;
int minTime;
this.constaintRange = Math.Abs(conflict.timeStepAgentB - conflict.timeStepAgentA);
if (constraintPolicy == Run.ConstraintPolicy.Range)
{
minTime = Math.Min(conflict.timeStepAgentA, conflict.timeStepAgentB);
}
else if (constraintPolicy == Run.ConstraintPolicy.DoubleRange)
{
minTime = conflict.timeStepAgentA;
}
else
{
if (agentA)
{
minTime = conflict.timeStepAgentA;
}
else
{
minTime = conflict.timeStepAgentB;
}
}
if (agentA)
{
move = conflict.agentAmove;
agentNum = instance.m_vAgents[conflict.agentAIndex].agent.agentNum;
this.move = new TimedMove(move, minTime);
}
else
{
move = conflict.agentBmove;
agentNum = instance.m_vAgents[conflict.agentBIndex].agent.agentNum;
this.move = new TimedMove(move, minTime);
}
this.agentNum = (byte)agentNum;
if (conflict.vertex)
{
this.move.direction = Move.Direction.NO_DIRECTION;
}
}
/// <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>
/// Setup the relevant data structures for a run.
/// </summary>
public virtual void Setup(ProblemInstance problemInstance, int minDepth, Run runner, int minCost)
{
this.instance = problemInstance;
this.runner = runner;
WorldState root = this.CreateSearchRoot(minDepth, minCost);
root.h = (int)this.heuristic.h(root); // g was already set in the constructor
this.openList.Add(root);
this.closedList.Add(root, root);
this.ClearPrivateStatistics();
this.generated++; // The root
this.totalCost = 0;
this.singleCosts = null;
this.solution = null;
this.singlePlans = null;
this.conflictCounts = null;
this.conflictTimes = null;
this.solutionDepth = -1;
this.numOfAgents = problemInstance.m_vAgents.Length;
// Store parameters used by IndependenceDetection's Independence Detection algorithm
if (problemInstance.parameters.ContainsKey("ID-max-cost"))
{
this.maxCost = (int)(problemInstance.parameters["ID-max-cost"]);
}
else
{
this.maxCost = int.MaxValue;
}
if (problemInstance.parameters.ContainsKey("ID - reserved") &&
((HashSet <TimedMove>)problemInstance.parameters["ID - reserved"]).Count != 0)
{
this.illegalMoves = (HashSet <TimedMove>)(problemInstance.parameters["ID - reserved"]);
}
else
{
this.illegalMoves = null;
}
if (problemInstance.parameters.ContainsKey(CBS.CONSTRAINTS) &&
((HashSet_U <CbsConstraint>)problemInstance.parameters[CBS.CONSTRAINTS]).Count != 0)
{
this.constraints = (HashSet_U <CbsConstraint>)problemInstance.parameters[CBS.CONSTRAINTS];
}
}
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
}
/// <summary>
/// Child from merge action constructor
/// </summary>
/// <param name="father"></param>
/// <param name="mergeGroupA"></param>
/// <param name="mergeGroupB"></param>
public CbsNode(CbsNode father, int mergeGroupA, int mergeGroupB)
{
this.allSingleAgentPlans = father.allSingleAgentPlans.ToArray <SinglePlan>();
this.allSingleAgentCosts = father.allSingleAgentCosts.ToArray <int>();
this.agentsGroupAssignment = father.agentsGroupAssignment.ToArray <ushort>();
this.MergeGroups(mergeGroupA, mergeGroupB);
this.prev = father;
this.constraint = null;
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;
}
/// <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
}
//public static double TIMEOUT = double.MaxValue;
/// <summary>
/// Write to file a given instance
/// </summary>
/// <param name="instance">The instance to execute</param>
public void WriteGivenProblem
(
ProblemInstance instance,
ISolver solver,
LinkedList <List <Move> > currentPlan = null)
{
int solutionSumOfCost;
if (currentPlan == null)
{
currentPlan = copyLinkedList(plan.GetLocations());
solutionSumOfCost = solutionCost;
}
else
{
solutionSumOfCost = computeSumOfCost(currentPlan);
}
double planTime = planningTime;
GC.Collect();
GC.WaitForPendingFinalizers();
string solverName = "";
if (solver.GetType() == typeof(CBS_LocalConflicts) || solver.GetType() == typeof(CBS_GlobalConflicts))
{
solverName = "ICBS + " + ((CBS_LocalConflicts)solver).conflictRange;
}
else
{
solverName = "EPEA*";
}
writeToFile(
solverName, // solver name
planTime.ToString(), // planning time
solutionSumOfCost.ToString(), // solution cost
instanceId.ToString(), // instanceId
instance.fileName, // file Name
instance.m_vAgents.Length.ToString(), // #Agents
m_mapFileName, // Map name
instance.m_nObstacles, // Obstacles
((CBS_GlobalConflicts)solver).constraintPolicy); // Constraint Policy
}
public void Setup(ProblemInstance instance, Run runner)
{
this.instance = instance;
this.runner = runner;
this.totalCost = 0;
this.ClearStatistics();
//this.accMaxGroupSize = 1;
this.conflictAvoidance = new Dictionary <TimedMove, List <int> >();
this.allConflicts = new HashSet <Conflict>();
this.allGroups = new LinkedList <AgentsGroup>();
// Initialize the agent group collection with a group for every agent
foreach (AgentState agentStartState in instance.m_vAgents)
{
this.allGroups.AddLast(new AgentsGroup(this.instance, new AgentState[1] {
agentStartState
}, this.singleAgentSolver, this.groupSolver));
}
}
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 long solve(CFMAStar.CostFunction costFunction)
{
// Independence Detection
List <List <TimedMove> > nonConflictsPaths = null;
IndependentDetection id = new IndependentDetection(this.problemInstance, this.goalState);
MAM_AgentState[] newStartPositions = id.Detect(out nonConflictsPaths);
if (newStartPositions.Length != 0)
{
this.problemInstance = problemInstance.ReplanProblem(newStartPositions);
this.reducer = new CFMAM_MCMF_Reducer(this.problemInstance, this.goalState);
reducer.reduce(costFunction);
if (reducer.outputProblem == null)
{
return(-1);
}
MinCostMaxFlow mcmfSolver = new MinCostMaxFlow(reducer.outputProblem);
timer = Stopwatch.StartNew();
solution = mcmfSolver.SolveMinCostFlow();
List <TimedMove>[] partialPlan = this.reducer.GetCFMAMSolution(this.solution, this.mcmfTime, true);
if (costFunction == CFMAStar.CostFunction.MakeSpan)
{
while (!isPathForEachAgent(partialPlan))
{
this.reducer.addNetworkLayer();
mcmfSolver = new MinCostMaxFlow(reducer.outputProblem);
solution = mcmfSolver.SolveMinCostFlow();
partialPlan = this.reducer.GetCFMAMSolution(this.solution, this.mcmfTime, true);
}
}
timer.Stop();
this.plan = mergePlans(partialPlan, nonConflictsPaths);
this.mcmfTime = timer.ElapsedMilliseconds;
this.solutionCost = calculateCost(this.plan, costFunction);
}
else
{
this.plan = nonConflictsPaths;
this.solutionCost = calculateCost(this.plan, costFunction);
}
return(this.solutionCost);
}
/// <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);
}
}
/// <summary>
/// Solve a given instance with the given solver
/// </summary>
/// <param name="solver">The solver</param>
/// <param name="instance">The problem instance that will be solved</param>
private void run
(
MAM_ISolver solver,
ProblemInstance instance,
HashSet <MMStarConstraint> constraints = null
)
{
// Run the algorithm
bool solved;
//Console.WriteLine("----------------- " + solver + " with " + solver.GetHeuristicCalculator().GetName() + ", Minimizing " + solver.GetCostFunction().ToString() + " -----------------");
this.startTime = this.ElapsedMillisecondsTotal();
solver.GetHeuristicCalculator().init(instance);
solver.Setup(instance, this);
if (constraints == null)
{
constraints = new HashSet <MMStarConstraint>();
}
//MMStarConstraint constraint1 = new MMStarConstraint(2, 3, 2, Move.Direction.South, 2);
//MMStarConstraint constraint2 = new MMStarConstraint(1, 1, 2, Move.Direction.NO_DIRECTION, 1);
//constraints.Add(constraint1);
//constraints.Add(constraint2);
solver.AddConstraints(constraints); // Add constraints
solved = solver.Solve();
elapsedTime = this.ElapsedMilliseconds();
if (solved)
{
//Console.WriteLine("Total MakeSpan cost: {0}", solver.GetSolutionMakeSpanCost());
//Console.WriteLine("Total SOC cost: {0}", solver.GetSolutionSOCCost());
}
else
{
Console.WriteLine("Failed to solve");
}
//Console.WriteLine();
//Console.WriteLine("Expanded nodes: {0}", solver.GetExpanded());
//Console.WriteLine("Time in milliseconds: {0}", elapsedTime);
if (toPrint)
{
this.PrintStatistics(instance, solver, elapsedTime);
}
}
public void Setup(ProblemInstance instance, Run runner)
{
this.instance = instance;
this.runner = runner;
this.expanded = 0;
this.generated = 0;
this.totalCost = 0;
this.maxGroup = 1;
this.minGroup = instance.m_vAgents.Length;
this.accMaxGroup = 1;
this.minGroup = instance.m_vAgents.Length;
this.conflictAvoidance = new Dictionary <TimedMove, List <int> >();
// Initialize the agent group collection with a group for every agent
foreach (AgentState agentStartState in instance.m_vAgents)
{
this.allGroups.AddFirst(new AgentsGroup(this.instance, new AgentState[1] {
agentStartState
}, this.groupSolver));
}
}
public CostTreeNodeSolver(ProblemInstance problem, CostTreeNode costNode, Run runner) //make sure agent numbers are in the correct order
{
this.runner = runner;
AgentState agent;
maxCost = 0;
allMDDs = new MDD[problem.GetNumOfAgents()];
this.startingPos = problem.m_vAgents;
this.costs = costNode.costs;
this.problem = problem;
maxCost = costNode.costs.Max();
for (int i = 0; i < startingPos.Length; i++)
{
agent = startingPos[i];
allMDDs[i] = new MDD(i, agent.agent.agentNum, agent.lastMove, costNode.costs[i], maxCost, startingPos.Length, problem);
}
matchCounter = 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;
}
private void GetSons(BFSNode node, ReducerOpenList <BFSNode> openList, ProblemInstance problem)
{
bool[][] problemGrid = problem.m_vGrid;
if (node.position.x != problemGrid.Length - 1 && !problemGrid[node.position.x + 1][node.position.y])
{
CollectSon(openList, node, node.position.x + 1, node.position.y);
}
if (node.position.x != 0 && !problemGrid[node.position.x - 1][node.position.y])
{
CollectSon(openList, node, node.position.x - 1, node.position.y);
}
if (node.position.y != problemGrid[node.position.x].Length - 1 && !problemGrid[node.position.x][node.position.y + 1])
{
CollectSon(openList, node, node.position.x, node.position.y + 1);
}
if (node.position.y != 0 && !problemGrid[node.position.x][node.position.y - 1])
{
CollectSon(openList, node, node.position.x, node.position.y - 1);
}
}
/// <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,
CFMAM_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 CFMAM_OpenList(this);
if (constraints != null)
{
this.constraints = constraints;
}
else
{
this.constraints = new HashSet <MMStarConstraint>();
}
// caculate lowest centrality
MAM_AgentState agent = getLowestCentralityAgent();
CalculateH(agent, null);
CalculateF(agent);
closed(agent);
openList.Add(agent);
}
private void PrintProblemStatistics(ProblemInstance instance)
{
// Grid Name col:
if (instance.parameters.ContainsKey(ProblemInstance.GRID_NAME_KEY))
{
this.resultsWriter.Write(instance.parameters[ProblemInstance.GRID_NAME_KEY] + RESULTS_DELIMITER);
}
else
{
this.resultsWriter.Write(RESULTS_DELIMITER);
}
// Grid Rows col:
this.resultsWriter.Write(instance.m_vGrid.Length + RESULTS_DELIMITER);
// Grid Columns col:
this.resultsWriter.Write(instance.m_vGrid[0].Length + RESULTS_DELIMITER);
// Num Of Agents col:
this.resultsWriter.Write(instance.m_vAgents.Length + RESULTS_DELIMITER);
// Num Of Obstacles col:
this.resultsWriter.Write(instance.m_nObstacles + RESULTS_DELIMITER);
// Instance Id col:
this.resultsWriter.Write(instance.instanceId + RESULTS_DELIMITER);
}
/// <summary>
/// Print the solver statistics to the results file.
/// </summary>
/// <param name="instance">The problem instance that was solved. Not used!</param>
/// <param name="solver">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, ISolver solver, double runtimeInMillis)
{
// Success col:
if (solver.GetSolutionCost() < 0)
{
this.resultsWriter.Write(Run.FAILURE_CODE + RESULTS_DELIMITER);
}
else
{
this.resultsWriter.Write(Run.SUCCESS_CODE + RESULTS_DELIMITER);
}
// Runtime col:
this.resultsWriter.Write(runtimeInMillis + RESULTS_DELIMITER);
// Solution Cost col:
this.resultsWriter.Write(solver.GetSolutionCost() + RESULTS_DELIMITER);
// Algorithm specific cols:
solver.OutputStatistics(this.resultsWriter);
// Max Group col:
this.resultsWriter.Write(solver.GetMaxGroupSize() + RESULTS_DELIMITER);
// Solution Depth col:
this.resultsWriter.Write(solver.GetSolutionDepth() + RESULTS_DELIMITER);
}
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>
/// Print the solver statistics to the results file.
/// </summary>
/// <param name="instance">The problem instance that was solved. Not used!</param>
/// <param name="solver">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,
MAM_ISolver solver,
double runtimeInMillis
)
{
// 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);
// Algorithm specific cols:
solver.OutputStatistics(this.resultsWriter);
// Solution Depth col:
this.resultsWriter.Write(solver.GetSolutionDepth() + RESULTS_DELIMITER);
//this.resultsWriter.Flush();
}
public CFMAM_MCMF_Reducer(ProblemInstance problem, Move goalState)
{
this.problemGrid = problem.m_vGrid;
startPositions = new Move[problem.GetNumOfAgents()];
this.startPositionsDict = new Dictionary <KeyValuePair <int, int>, int>();
for (int i = 0; i < problem.m_vAgents.Length; i++)
{
startPositions[i] = problem.m_vAgents[i].lastMove;
this.startPositionsDict.Add(new KeyValuePair <int, int>(startPositions[i].x, startPositions[i].y), 1);
}
startPositionsToDiscover = problem.GetNumOfAgents();
this.goalState = goalState;
this.NFNodes = new HashSet <NFReducerNode>();
this.l = -1;
this.T = -1;
this.edgeCounter = 0;
this.outputProblem = null;
NFReducerNode.indexCounter = 0;
this.zeroLayer = new List <NFReducerNode>();
}
} // Nonsense values until Init, just allocate move
public CFMCbsConstraint(CFMCbsConflict conflict, ProblemInstance instance, bool agentA)
{
Move move;
int agentNum;
int minTime;
if (agentA)
{
minTime = conflict.timeStepAgentA;
}
else
{
minTime = conflict.timeStepAgentB;
}
if (agentA)
{
move = conflict.agentAmove;
agentNum = instance.m_vAgents[conflict.agentAIndex].agentIndex;
this.move = new TimedMove(move, minTime);
}
else
{
move = conflict.agentBmove;
agentNum = instance.m_vAgents[conflict.agentBIndex].agentIndex;
this.move = new TimedMove(move, minTime);
}
this.agentNum = (byte)agentNum;
if (conflict.vertex)
{
this.move.direction = Move.Direction.NO_DIRECTION;
}
}
} // Nonsense values until Init, just allocate move
public CbsConstraint(CbsConflict conflict, ProblemInstance instance, bool agentA)
{
Move move;
int agentNum;
if (agentA)
{
move = conflict.agentAmove;
agentNum = instance.m_vAgents[conflict.agentA].agent.agentNum;
}
else
{
move = conflict.agentBmove;
agentNum = instance.m_vAgents[conflict.agentB].agent.agentNum;
}
this.agent = (byte)agentNum;
this.move = new TimedMove(move, conflict.timeStep);
if (conflict.vertex)
{
this.move.direction = Move.Direction.NO_DIRECTION;
}
}
/// <summary>
/// Determines how many additive pattern databases to build and divides
/// the agents among them, possibly leaving some agents out.
/// </summary>
/// <param name="s">The root of the search tree. This is also expected
/// to have context parameters such as agents' goal states.</param>
public void build(ProblemInstance pi, WorldState s)
{
Debug.Write("Building database...");
/**
* As a simple rule, we'll simply take pairs of agents starting
* with the first two, then the second two, etc.
*/
m_vPDBs = new List <PDB>();
if (s.allAgentsState.Length > 1)
{
for (uint i = 0; i < s.allAgentsState.Length - 1; i += 2)
{
/**
* Make a list of agents we want to include together in the
* next additive pattern database. We specify agents by
* their index into the Travor_WorldState.allAgentsState
* array.
*/
List <uint> vAgents = new List <uint>();
vAgents.Add(i);
vAgents.Add(i + 1);
/**
* Create a new root search node where the state only
* includes a subset of the agents of the original search
* node. This is done by passing into the state copy
* constructor our list of important agents.
*/
WorldState tws = new WorldState(s.allAgentsState, vAgents);
/**
* Initialize, build, and save the new pattern database.
*/
EnumeratedPDB pdb = new EnumeratedPDB();
pdb.init(pi, vAgents);
pdb.build();
Debug.Write(".");
m_vPDBs.Add(pdb);
}
}
/**
* Create single shortest path pattern database heuristics for the
* remaining agents if we have any left over.
*/
if (s.allAgentsState.Length % 2 == 1)
{
SumIndividualCosts pdb = new SumIndividualCosts();
List <uint> vAgents = new List <uint>(1);
vAgents.Add((uint)s.allAgentsState.Length - 1);
pdb.init(pi, vAgents);
pdb.build();
m_vPDBs.Add(pdb);
}
/**
* For informational purposes, we will set the number of agents
* that aren't included in this set of pattern databases.
*/
m_vExcludedAgents = new SortedSet <uint>();
Debug.WriteLine("done.");
}
/// <summary>
/// Initializes the pattern database by storing references to the
/// problem instance and also the subset of agents that the pattern
/// database pertains to.
/// </summary>
/// <param name="pi">The problem instance.</param>
/// <param name="vAgents">The agents that the pattern database should keep track of.</param>
public virtual void init(ProblemInstance pi, List <uint> vAgents)
{
}
/// <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);
}
/// <summary>
///
/// </summary>
/// <param name="mddNum"></param>
/// <param name="agentNum"></param>
/// <param name="start_pos"></param>
/// <param name="cost">The MDD must be of this cost</param>
/// <param name="numOfLevels">
/// The MDD must be of this number of levels, not counting level zero.
/// If higher than cost, the extra levels will be WAITs at the goal.
/// </param>
/// <param name="numOfAgents"></param>
/// <param name="instance"></param>
/// <param name="ignoreConstraints"></param>
public MDD(int mddNum, int agentNum, Move start_pos, int cost, int numOfLevels, int numOfAgents, ProblemInstance instance, bool ignoreConstraints = false, bool supportPruning = true)
{
this.problem = instance;
this.mddNum = mddNum;
this.agentNum = agentNum;
this.cost = cost;
this.levels = new LinkedList <MDDNode> [numOfLevels + 1];
this.supportPruning = supportPruning;
if (ignoreConstraints == false && instance.parameters.ContainsKey(CBS_LocalConflicts.CONSTRAINTS) &&
((HashSet_U <CbsConstraint>)instance.parameters[CBS_LocalConflicts.CONSTRAINTS]).Count != 0)
{
this.queryConstraint = new CbsConstraint();
this.queryConstraint.queryInstance = true;
this.constraints = (HashSet_U <CbsConstraint>)instance.parameters[CBS_LocalConflicts.CONSTRAINTS];
}
if (ignoreConstraints == false && instance.parameters.ContainsKey(CBS_LocalConflicts.MUST_CONSTRAINTS) &&
((HashSet_U <CbsConstraint>)instance.parameters[CBS_LocalConflicts.MUST_CONSTRAINTS]).Count != 0)
{
// TODO: Code dup with ClassicAStar's constructor
var musts = (HashSet_U <CbsConstraint>)instance.parameters[CBS_LocalConflicts.MUST_CONSTRAINTS];
this.mustConstraints = new Dictionary <int, TimedMove> [musts.Max <CbsConstraint>(con => con.GetTimeStep()) + 1]; // To have index MAX, array needs MAX + 1 places.
foreach (CbsConstraint con in musts)
{
int timeStep = con.GetTimeStep();
if (this.mustConstraints[timeStep] == null)
{
this.mustConstraints[timeStep] = new Dictionary <int, TimedMove>();
}
this.mustConstraints[timeStep][con.agentNum] = con.move;
}
}
var closedList = new Dictionary <MDDNode, MDDNode>();
var toDelete = new List <MDDNode>();
for (int i = 0; i <= numOfLevels; i++)
{
levels[i] = new LinkedList <MDDNode>();
}
MDDNode root = new MDDNode(new TimedMove(start_pos, 0), numOfAgents, this, supportPruning); // Root
LinkedListNode <MDDNode> llNode = new LinkedListNode <MDDNode>(root);
root.setMyNode(llNode);
llNode.Value.startOrGoal = true;
levels[0].AddFirst(llNode);
for (int i = 0; i < numOfLevels; i++) // For each level, populate the _next_ level
{
int heuristicBound = cost - i - 1; // We want g+h <= cost, so h <= cost-g. -1 because it's the bound of the _children_.
if (heuristicBound < 0)
{
heuristicBound = 0;
}
// Go over each MDDNode in this level
foreach (MDDNode currentMddNode in levels[i]) // Since we're not deleting nodes in this method, we can use the simpler iteration method :)
{
List <MDDNode> children = this.GetAllChildren(currentMddNode, heuristicBound, numOfAgents);
if (children.Count == 0) // Heuristic wasn't perfect because of constraints, illegal moves or other reasons
{
toDelete.Add(currentMddNode);
}
foreach (MDDNode child in children)
{
MDDNode toAdd = child; // The compiler won't let me assign to the foreach variable...
if (closedList.ContainsKey(child))
{
toAdd = closedList[child];
}
else
{
closedList.Add(toAdd, toAdd);
llNode = new LinkedListNode <MDDNode>(toAdd);
toAdd.setMyNode(llNode);
levels[i + 1].AddLast(toAdd);
}
currentMddNode.addChild(toAdd); // forward edge
toAdd.addParent(currentMddNode); // backward edge
}
}
closedList.Clear();
}
foreach (MDDNode goal in levels[numOfLevels]) // The goal may be reached in more than one direction
{
goal.startOrGoal = true;
}
foreach (MDDNode remove in toDelete)
{
remove.delete();
}
// Make sure the goal was reached - imperfect heuristics, constraints or illegal moves can cause this to be false.
if (levels[numOfLevels].Count == 0 || levels[0].First.Value.isDeleted == true) //if no possible route mark levels as null
{
levels = null;
}
}
public MDD(int mddNum, int agentNum, Move start_pos, int cost, int maxCostOnLevel, int numOfAgents, ProblemInstance instance)
{
//if (agentNum == 2 && maxCostOnLevel == 4)
// Console.Write("ff");
this.problem = instance;
this.mddNum = mddNum;
this.agentNum = agentNum;
levels = new LinkedList <MDDNode> [maxCostOnLevel + 1];
Hashtable closedList = new Hashtable();
LinkedList <MDDNode> children;
LinkedList <MDDNode> toDelete = null;
for (int i = 0; i <= maxCostOnLevel; i++)
{
levels[i] = new LinkedList <MDDNode>();
}
MDDNode toAdd = new MDDNode(new TimedMove(start_pos, 0), numOfAgents, this);
LinkedListNode <MDDNode> llNode = new LinkedListNode <MDDNode>(toAdd);
toAdd.setMyNode(llNode);
llNode.Value.startOrGoal = true;
levels[0].AddFirst(llNode);
for (int i = 0; i < maxCostOnLevel; i++)
{
int heuristicBound = cost - i - 1;
if (heuristicBound < 0)
{
heuristicBound = 0;
}
LinkedListNode <MDDNode> currentMddNode = levels[i].First;
while (currentMddNode != null)
{
LinkedListNode <MDDNode> child;
children = this.GetAllChildren(currentMddNode.Value, heuristicBound, numOfAgents, i);
child = children.First;
if (child == null)
{
if (toDelete == null)
{
toDelete = new LinkedList <MDDNode>();
}
toDelete.AddFirst(currentMddNode.Value);
}
while (child != null)
{
toAdd = child.Value;
if (closedList.Contains(toAdd))
{
toAdd = (MDDNode)closedList[toAdd];
}
else
{
closedList.Add(toAdd, toAdd);
llNode = new LinkedListNode <MDDNode>(toAdd);
toAdd.setMyNode(llNode);
levels[i + 1].AddLast(llNode);
}
currentMddNode.Value.addChild(toAdd);
toAdd.addParent(currentMddNode.Value);
child = child.Next;
}
currentMddNode = currentMddNode.Next;
}
closedList.Clear();
}
if (levels[maxCostOnLevel].Count != 0)
{
levels[maxCostOnLevel].First.Value.startOrGoal = true;
}
if (toDelete != null)
{
foreach (MDDNode remove in toDelete)
{
remove.delete();
}
}
if (levels[maxCostOnLevel].Count == 0 || levels[0].First.Value.isDeleted == true) //if no possible route mark levels as null
{
levels = null;
}
}