Exemplo n.º 1
0
        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}!");
                        }
                    }
                }
            }
        }
Exemplo n.º 2
0
        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);
        }
Exemplo n.º 3
0
        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);
        }
Exemplo n.º 4
0
 /// <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]);
     }
 }
Exemplo n.º 5
0
 /// <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);
 }
Exemplo n.º 6
0
        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));
        }
Exemplo n.º 7
0
 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);
            }
        }
Exemplo n.º 10
0
        /// <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);
        }
Exemplo n.º 11
0
        /// <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));
        }
Exemplo n.º 12
0
        /// <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]);
            }
        }
Exemplo n.º 13
0
        /// <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);
            }
        }
Exemplo n.º 14
0
 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;
 }