/// <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);
}