public void Check(ProblemInstance problem) { SinglePlan[] singles = new SinglePlan[this.locationsAtTimes.First().Count]; for (int i = 0; i < singles.Length; i++) { singles[i] = new SinglePlan(this, i, problem.agents[i].agent.agentNum); foreach ((int time, var move) in singles[i].locationAtTimes.Enumerate()) { Trace.Assert(problem.IsValid(move), $"Plan of agent {i} uses an invalid location {move} at time {time}!"); } } // Check in every time step that the plans do not collide for (int time = 1; time < this.locationsAtTimes.Count; time++) // Assuming no conflicts exist in time zero. { // Check all pairs of agents for a collision at the given time step foreach ((int i1, var plan1) in singles.Enumerate()) { foreach ((int i2, var plan2) in singles.Enumerate()) { if (i1 < i2) { Trace.Assert(plan1.IsColliding(time, plan2) == false, $"Plans of agents {i1} and {i2} collide at time {time}!"); } } } } }
public static SinglePlan[] GetSinglePlans(WorldState goalState) // FIXME: Duplication with other methods. { LinkedList <Move>[] allroutes = new LinkedList <Move> [goalState.allAgentsState.Length]; for (int i = 0; i < allroutes.Length; i++) { allroutes[i] = new LinkedList <Move>(); } WorldState currentNode = goalState; while (currentNode != null) { for (int i = 0; i < allroutes.Length; i++) { allroutes[i].AddFirst(currentNode.GetSingleAgentMove(i)); } currentNode = currentNode.prevStep; } SinglePlan[] ans = new SinglePlan[goalState.allAgentsState.Length]; for (int i = 0; i < ans.Length; i++) { ans[i] = new SinglePlan(allroutes[i], goalState.allAgentsState[i].agent.agentNum); } return(ans); }
private SinglePlan[] GetAnswer(A_Star_MDDs_Node finish) { // TODO: Move the construction of the SinglePlans to a static method in SinglePlan var routes = new LinkedList <Move> [problem.Length]; for (int i = 0; i < routes.Length; i++) { routes[i] = new LinkedList <Move>(); } A_Star_MDDs_Node current = finish; while (current != null) { for (int i = 0; i < problem.Length; i++) { routes[i].AddFirst(new Move(current.allSteps[i].move)); } current = current.prev; } var ans = new SinglePlan[problem.Length]; for (int i = 0; i < ans.Length; i++) { ans[i] = new SinglePlan(routes[i], i); } return(ans); }
/// <summary> /// Set the optimal solution of this node as a problem instance. /// Currently only used by CbsHeuristicForAStar, if a solution was found while running the heuristic. /// </summary> /// <param name="solution"></param> public virtual void SetSolution(SinglePlan[] solution) { this.singlePlans = SinglePlan.GetSinglePlans(this); // This node may be a partial solution itself, need to start from the real root. for (int i = 0; i < solution.Length; ++i) { this.singlePlans[i].ContinueWith(solution[i]); } }
/// <summary> /// Creates SinglePlans with agentIndex as agentNum. Not suitable for subproblems. /// </summary> /// <param name="allRoutes"></param> /// <returns></returns> public static SinglePlan[] GetSinglePlans(LinkedList <Move>[] allRoutes) { SinglePlan[] ans = new SinglePlan[allRoutes.Length]; for (int i = 0; i < ans.Length; i++) { ans[i] = new SinglePlan(allRoutes[i], i); } return(ans); }
public override bool Equals(object obj) { if (obj == null) { return(false); } SinglePlan other = (SinglePlan)obj; return(this.agentNum == other.agentNum && this.locationAtTimes.SequenceEqual <Move>(other.locationAtTimes)); }
public SinglePlan[] GetSinglePlans() { if (this.singlePlans != null) { return(this.singlePlans); } else { return(SinglePlan.GetSinglePlans(this)); } }
public void removeGroupFromCAT(ConflictAvoidanceTable CAT) { if (this.plan == null) { return; } for (int i = 0; i < this.allAgentsState.Length; i++) { var singleAgentPlan = new SinglePlan(this.plan, i, this.groupNum); CAT.RemovePlan(singleAgentPlan); } }
public void addGroupToCAT(ConflictAvoidanceTable CAT) { if (this.plan == null) { return; } for (int i = 0; i < this.allAgentsState.Length; i++) { var singleAgentPlan = new SinglePlan(this.plan, i, this.groupNum); // Note all the plans are inserted under the group's identifier CAT.AddPlan(singleAgentPlan); } }
/// <summary> /// Check if this plan collides with another plan at a given time /// </summary> /// <param name="time">The time at which to check if the collision occured</param> /// <param name="otherPlan">The plan to check against</param> public bool IsColliding(int time, SinglePlan otherPlan) { Move thisLocation = this.GetLocationAt(time); Move otherLocation = otherPlan.GetLocationAt(time); if (thisLocation.IsColliding(otherLocation) == true) // IsColliding isn't virtual, // so it doesn't matter whether the moves are actually TimedMoves // with incorrect time { return(true); } return(false); }
/// <summary> /// Calculate the full plan for all the agents that has been found by the algorithm /// </summary> public Plan CalculateJointPlan() { var singlePlans = new SinglePlan[this.instance.GetNumOfAgents()]; foreach (var group in this.allGroups) { var groupPlan = group.GetPlan(); int i = 0; foreach (var agentState in group.allAgentsState) { singlePlans[agentState.agent.agentNum] = new SinglePlan(groupPlan, i, agentState.agent.agentNum); i++; } } return(new Plan(singlePlans)); }
/// <summary> /// Set the optimal solution of this node as a problem instance. /// </summary> /// <param name="solution"></param> public override void SetSolution(SinglePlan[] solution) { if (this.agentTurn == 0) { this.singlePlans = SinglePlan.GetSinglePlans(this); } else { this.singlePlans = SinglePlan.GetSinglePlans(this.prevStep); } // ToProblemInstance gives the last proper state as the problem to solve, // with must constraints to make the solution go through the steps already // taken from there. for (int i = 0; i < solution.Length; ++i) { this.singlePlans[i].ContinueWith(solution[i]); } }
/// <summary> /// Add actions of other plan after actions of plan. /// If this plan ends where the other starts, /// the first timestep of the other plan is skipped /// </summary> /// <param name="other"></param> public void ContinueWith(SinglePlan other) { bool first = true; foreach (Move newLocationAtTime in other.locationAtTimes) { if (first) { first = false; if (this.locationAtTimes[this.locationAtTimes.Count - 1].Equals(newLocationAtTime)) { continue; } else { Trace.Assert(false, "Continuing a plan doesn't start from the same state"); } } this.locationAtTimes.Add(newLocationAtTime); } }
public SinglePlan(SinglePlan cpy) { this.locationAtTimes = cpy.locationAtTimes.ToList <Move>(); // Behavior change: used to do a deep copy, with cloned moves. this.agentNum = cpy.agentNum; }