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) { }