public virtual ProblemInstance ToProblemInstance(ProblemInstance initial)
{
// Notice this is not a subproblem in the number of agents but
// in the steps from the start.
// It might even be harder if the steps were away from the goal.
return(initial.Subproblem(this.allAgentsState));
}
public AgentsGroup(ProblemInstance instance, AgentState[] allAgentsState, ISolver singleAgentSolver, ISolver groupSolver)
{
this.allAgentsState = allAgentsState;
this.instance = instance.Subproblem(allAgentsState);
this.singleAgentSolver = singleAgentSolver;
this.groupSolver = groupSolver;
}
public override ProblemInstance ToProblemInstance(ProblemInstance initial)
{
WorldState state = this;
if (this.agentTurn != 0)
{
// CBS doesn't handle partially expanded nodes well.
// Use the last fully expanded node and add the additional moves as must conds:
state = this.prevStep; // Points to the last fully expanded node.
}
ProblemInstance subproblem = initial.Subproblem(state.allAgentsState); // Can't use base's method because we're operating on a different object
if (this.agentTurn != 0)
{
subproblem.parameters = new Dictionary <string, object>(subproblem.parameters); // Use a copy to not pollute general problem instance with the must constraints
if (subproblem.parameters.ContainsKey(CBS_LocalConflicts.MUST_CONSTRAINTS) == false)
{
subproblem.parameters[CBS_LocalConflicts.MUST_CONSTRAINTS] = new List <CbsConstraint>();
}
var mustConstraints = (List <CbsConstraint>)subproblem.parameters[CBS_LocalConflicts.MUST_CONSTRAINTS];
for (int i = 0; i < this.agentTurn; ++i)
{
mustConstraints.Add(new CbsConstraint(this.allAgentsState[i].agent.agentNum, this.allAgentsState[i].lastMove));
}
}
return(subproblem);
}
/// <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 AgentsGroup(ProblemInstance instance, AgentState[] allAgentsState, ISolver solver)
{
this.allAgentsState = allAgentsState;
this.instance = instance.Subproblem(allAgentsState);
this.solver = solver;
}
