Exemplo n.º 1
0
        /// <summary>
        ///
        /// </summary>
        /// <param name="problemInstance"></param>
        /// <param name="minDepth"></param>
        /// <param name="runner"></param>
        /// <param name="minCost">Not taken into account</param>
        public virtual void Setup(ProblemInstance problemInstance, int minDepth, Run runner, int minCost = -1)
        {
            this.instance = problemInstance;
            this.runner   = runner;
            this.ClearPrivateStatistics();
            this.totalCost            = 0;
            this.solutionDepth        = -1;
            this.targetCost           = int.MaxValue;
            this.lowLevelGeneratedCap = int.MaxValue;
            this.milliCap             = int.MaxValue;
            this.goalNode             = null;
            this.solution             = null;

            this.maxCost = int.MaxValue;
            this.topMost = this.SetGlobals();

            this.minDepth = minDepth;
            CbsNode root = new CbsNode(instance.m_vAgents.Length, this.solver, this.singleAgentSolver, this); // Problem instance and various strategy data is all passed under 'this'.
            // Solve the root node
            bool solved = root.Solve(minDepth);

            if (solved && root.totalCost <= this.maxCost)
            {
                this.openList.Add(root);
                this.highLevelGenerated++;
                this.closedList.Add(root, root);
            }
        }
Exemplo n.º 2
0
        public virtual void Expand(CbsNode node)
        {
            ushort          parentCost     = node.totalCost;
            ushort          parentH        = node.h;
            IList <CbsNode> children       = null;  // To quiet the compiler
            bool            reinsertParent = false; // To quiet the compiler


            this.ExpandImpl(node, out children, out reinsertParent);


            // Both children considered. None adopted. Add them to the open list, and re-insert the partially expanded parent too if necessary.
            if (reinsertParent)
            {
                this.openList.Add(node); // Re-insert node into open list with higher cost, don't re-increment global conflict counts
            }
            foreach (var child in children)
            {
                closedList.Add(child, child);

                if (child.totalCost == parentCost) // Total cost didn't increase (yet)
                {
                    child.h = parentH;
                }

                if (child.totalCost <= this.maxCost)
                {
                    this.highLevelGenerated++;
                    openList.Add(child);
                }
            }
        }
Exemplo n.º 3
0
        public bool DoesMustConstraintAllow(CbsConstraint check)
        {
            CbsNode current = this;

            while (current != null)
            {
                if (current.mustConstraint != null && !current.mustConstraint.Allows(check))
                {
                    return(false);
                }
                current = current.prev;
            }
            return(true);
        }
Exemplo n.º 4
0
        public int CompareTo(IBinaryHeapItem item)
        {
            CbsNode other = (CbsNode)item;

            if (this.totalCost < other.totalCost)
            {
                return(-1);
            }
            if (this.totalCost > other.totalCost)
            {
                return(1);
            }

            // Tie breaking:
            // We could prefer less external conflicts, even over goal nodes, as goal nodes with less external conflicts are better.
            // External conflicts are already taken into account by the low level solver to prefer less conflicts between fewer agents.
            // This would only help when this CBS is used as a low level solver, but is very costly to compute, and is computed also
            // for high level CBS solvers, so I removed it.

            // Internal conflict counts are ignored. 100 conflicts between the same two agents can possibly be solved by a single replan,
            // while two conflicts between two sets of agents take two replans to solve.

            // Prefer goal nodes. The elaborate form is to keep the comparison consistent. Without it goalA<goalB and also goalB<goalA.
            if (this.GoalTest() == true && other.GoalTest() == false)
            {
                return(-1);
            }
            if (other.GoalTest() == true && this.GoalTest() == false)
            {
                return(1);
            }

            // Not preferring more depth because it makes no sense. It's not like preferring larger g,
            // which is smart because that part of the cost isn't an estimate.

            // Prefer partially expanded nodes. They're less work because they have less constraints and only one child to generate.
            // The elaborate form, again, is to keep the comparison consistent. Without it partiallyExpandedA<partiallyExpandedB and partiallyExpandedA>partiallyExpandedB
            if ((this.agentAExpansion == CbsNode.ExpansionState.DEFERRED || this.agentBExpansion == CbsNode.ExpansionState.DEFERRED) &&
                other.agentAExpansion == CbsNode.ExpansionState.NOT_EXPANDED && other.agentBExpansion == CbsNode.ExpansionState.NOT_EXPANDED)
            {
                return(-1);
            }
            if ((other.agentAExpansion == CbsNode.ExpansionState.DEFERRED || other.agentBExpansion == CbsNode.ExpansionState.DEFERRED) &&
                this.agentAExpansion == CbsNode.ExpansionState.NOT_EXPANDED && this.agentBExpansion == CbsNode.ExpansionState.NOT_EXPANDED)
            {
                return(1);
            }
            return(0);
        }
Exemplo n.º 5
0
        public List <CbsConstraint> GetMustConstraints()
        {
            var     constraints = new List <CbsConstraint>();
            CbsNode current     = this;

            while (current.depth > 0)
            {
                if (current.mustConstraint != null)
                {
                    constraints.Add(current.mustConstraint);
                }
                current = current.prev;
            }
            constraints.Sort();
            return(constraints);
        }
Exemplo n.º 6
0
 /// <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;
 }
Exemplo n.º 7
0
        public void Setup(ProblemInstance problemInstance, Run runner)
        {
            AgentState.EquivalenceOverDifferentTimes = false;
            globalConflictsCounter = new int[problemInstance.m_vAgents.Length][];
            for (int i = 0; i < globalConflictsCounter.Length; i++)
            {
                globalConflictsCounter[i] = new int[i];
                for (int j = 0; j < i; j++)
                {
                    globalConflictsCounter[i][j] = 0;
                }
            }

            this.instance           = problemInstance;
            this.runner             = runner;
            root                    = new CbsNode(instance.m_vAgents.Length, problemInstance, this.solver, this.lowLevelSolver, runner);
            this.highLevelExpanded  = 0;
            this.highLevelGenerated = 1;
            maxSizeGroup            = 1;
            this.totalCost          = 0;

            if (problemInstance.parameters.ContainsKey(Trevor.MAXIMUM_COST_KEY))
            {
                this.maxCost = (int)(problemInstance.parameters[Trevor.MAXIMUM_COST_KEY]);
            }
            else
            {
                this.maxCost = int.MaxValue;
            }

            if (problemInstance.parameters.ContainsKey(CBS_LocalConflicts.INTERNAL_CAT) == false) // Top-most CBS only
            {
                problemInstance.parameters[CBS_LocalConflicts.INTERNAL_CAT]     = new HashSet_U <TimedMove>();
                problemInstance.parameters[CBS_LocalConflicts.CONSTRAINTS]      = new HashSet_U <CbsConstraint>();
                problemInstance.parameters[CBS_LocalConflicts.MUST_CONSTRAINTS] = new List <CbsConstraint>();
                this.topMost = true;
            }
            else
            {
                this.topMost = false;
            }

            minCost = 0;
        }
Exemplo n.º 8
0
 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;
 }
Exemplo n.º 9
0
        public HashSet <CbsConstraint> GetConstraints()
        {
            var     constraints = new HashSet <CbsConstraint>();
            CbsNode current     = this;

            while (current.depth > 0) // The root has no constraints
            {
                if (this.agentsGroupAssignment[current.prev.conflict.agentA] !=
                    this.agentsGroupAssignment[current.prev.conflict.agentB]) // Ignore constraints that deal with conflicts between
                                                                              // agents that were later merged. They're irrelevant
                                                                              // since merging fixes all conflicts between merged agents.
                                                                              // Nodes that only differ in such irrelevant conflicts will have the same single agent paths.
                                                                              // Dereferencing current.prev is safe because current isn't the root.
                {
                    constraints.Add(current.constraint);
                }
                current = current.prev;
            }
            return(constraints);
        }
Exemplo n.º 10
0
        public virtual void Setup(ProblemInstance problemInstance, int minDepth, Run runner)
        {
            this.instance = problemInstance;
            this.runner   = runner;

            this.ClearPrivateStatistics();
            this.totalCost            = 0;
            this.solutionDepth        = -1;
            this.targetCost           = int.MaxValue;
            this.lowLevelGeneratedCap = int.MaxValue;
            this.milliCap             = int.MaxValue;
            this.goalNode             = null;
            this.solution             = null;

            if (problemInstance.parameters.ContainsKey(Trevor.MAXIMUM_COST_KEY))
            {
                this.maxCost = (int)(problemInstance.parameters[Trevor.MAXIMUM_COST_KEY]);
            }
            else
            {
                this.maxCost = int.MaxValue;
            }

            this.topMost = this.SetGlobals();

            this.minDepth = minDepth;

            CbsNode root = new CbsNode(instance.m_vAgents.Length, problemInstance, this.solver, this.singleAgentSolver, runner);
            // Solve the root node
            bool solved = root.Solve(minDepth);

            if (solved && root.totalCost <= this.maxCost)
            {
                this.openList.Add(root);
                this.highLevelGenerated++;
                this.closedList.Add(root, root);
                this.addToGlobalConflictCount(root.GetConflict());
            }
        }
Exemplo n.º 11
0
        /// <summary>
        /// Merge agent groups that are conflicting in this node if they pass the merge threshold.
        /// </summary>
        /// <param name="mergeThreshold"></param>
        /// <returns>Whether a merge was performed.</returns>
        public bool ShouldMerge(int mergeThreshold)
        {
            int        countConflicts = 1; // The agentA and agentB conflict in this node.
            ISet <int> firstGroup     = this.GetGroup(this.agentsGroupAssignment[this.conflict.agentA]);
            ISet <int> secondGroup    = this.GetGroup(this.agentsGroupAssignment[this.conflict.agentB]);

            CbsNode current = this.prev;
            int     a, b;

            while (current != null)
            {
                a = current.conflict.agentA;
                b = current.conflict.agentB;
                if ((firstGroup.Contains(a) && secondGroup.Contains(b)) || (firstGroup.Contains(b) && secondGroup.Contains(a)))
                {
                    countConflicts++;
                }
                current = current.prev;
            }

            return(countConflicts > mergeThreshold);
        }
Exemplo n.º 12
0
        /// <summary>
        /// Checks the group assignment and the constraints
        /// </summary>
        /// <param name="obj"></param>
        /// <returns></returns>
        public override bool Equals(object obj)
        {
            CbsNode other = (CbsNode)obj;

            if (this.agentsGroupAssignment.SequenceEqual <ushort>(other.agentsGroupAssignment) == false)
            {
                return(false);
            }

            CbsNode current = this;
            HashSet <CbsConstraint> other_constraints = other.GetConstraints();
            HashSet <CbsConstraint> constraints       = this.GetConstraints();

            foreach (CbsConstraint constraint in constraints)
            {
                if (other_constraints.Contains(constraint) == false)
                {
                    return(false);
                }
                current = current.prev;
            }
            return(constraints.Count == other_constraints.Count);
        }
Exemplo n.º 13
0
        protected CbsNode ConstraintExpand(CbsNode node, bool doLeftChild, out int closedListHitChildCost)
        {
            CbsConflict conflict = node.GetConflict();
            int         conflictingAgentIndex = doLeftChild? conflict.agentAIndex : conflict.agentBIndex;

            CbsNode.ExpansionState expansionsState           = doLeftChild ? node.agentAExpansion : node.agentBExpansion;
            CbsNode.ExpansionState otherChildExpansionsState = doLeftChild ? node.agentBExpansion : node.agentAExpansion;
            string agentSide = doLeftChild? "left" : "right";
            int    planSize  = node.allSingleAgentPlans[conflictingAgentIndex].GetSize();
            int    groupSize = node.GetGroupSize(conflictingAgentIndex);

            closedListHitChildCost = -1;

            if ((Constants.Variant == Constants.ProblemVariant.ORIG &&
                 expansionsState == CbsNode.ExpansionState.NOT_EXPANDED && conflict.vertex == true &&
                 conflict.timeStep >= node.allSingleAgentCosts[conflictingAgentIndex] &&             // TODO: Can't just check whether the node is at its goal - the plan may involve it passing through its goal and returning to it later because of preexisting constraints.
                 node.h < conflict.timeStep + 1 - node.allSingleAgentCosts[conflictingAgentIndex] && // Otherwise we won't be increasing its h and there would be no reason to delay expansion
                 groupSize == 1) ||                                                                  // Otherwise an agent in the group can be forced to take a longer route without increasing the group's cost because another agent would be able to take a shorter route.
                (Constants.Variant == Constants.ProblemVariant.NEW &&
                 expansionsState == CbsNode.ExpansionState.NOT_EXPANDED && conflict.vertex == true &&
                 ((conflict.timeStep > planSize - 1 && node.h < 2) ||
                  (conflict.timeStep == planSize - 1 && node.h < 1)) &&
                 groupSize == 1)) // Otherwise an agent in the group can be forced to take a longer route without increasing the group's cost because another agent would be able to take a shorter route.
            // Conflict happens when or after the agent reaches its goal, and the agent is in a single-agent group.
            // With multi-agent groups, banning the goal doesn't guarantee a higher cost solution,
            // since if an agent is forced to take a longer route it may enable another agent in the group
            // to take a shorter route, getting an alternative solution of the same cost
            // The child would cost a lot because:
            // A) All WAIT moves in the goal before leaving it now add to the g (if we're in the original problem variant).
            // B) We force the low level to compute a path longer than the optimal,
            //    and with a bad suprise towards the end in the form of a constraint,
            //    so the low-level's SIC heuristic performs poorly.
            // C) We're banning the GOAL from all directions (since this is a vertex conflict),
            //    so any alternative plan will at least cost 1 more.
            //    We're ignoring edge conflicts because they can only happen at the goal when reaching it,
            //    and aren't guaranteed to increase the cost because the goal can still be possibly reached from another edge.
            {
                if (otherChildExpansionsState == CbsNode.ExpansionState.DEFERRED)
                {
                    throw new Exception("Unexpected: Expansion of both children deffered, but this is a vertex conflict so that means the targets for the two agents are equal, which is illegal");
                }

                if (debug)
                {
                    Debug.WriteLine("Skipping " + agentSide + " child for now");
                }
                if (doLeftChild)
                {
                    node.agentAExpansion = CbsNode.ExpansionState.DEFERRED;
                }
                else
                {
                    node.agentBExpansion = CbsNode.ExpansionState.DEFERRED;
                }
                // Add the minimal delta in the child's cost:
                // since we're banning the goal at conflict.timeStep, it must at least do conflict.timeStep+1 steps
                if (Constants.Variant == Constants.ProblemVariant.ORIG)
                {
                    node.h = (ushort)(conflict.timeStep + 1 - node.allSingleAgentCosts[conflictingAgentIndex]);
                }
                else if (Constants.Variant == Constants.ProblemVariant.NEW)
                {
                    if (conflict.timeStep > planSize - 1) // Agent will need to step out and step in to the goal, at least
                    {
                        node.h = 2;
                    }
                    else // Conflict is just when agent enters the goal, it'll have to at least wait one timestep.
                    {
                        node.h = 1;
                    }
                }
                return(node);
            }
            else if (expansionsState != CbsNode.ExpansionState.EXPANDED)
            // Agent expansion already skipped in the past or not forcing it from its goal - finally generate the child:
            {
                if (debug)
                {
                    Debug.WriteLine("Generating " + agentSide + " child");
                }

                if (doLeftChild)
                {
                    node.agentAExpansion = CbsNode.ExpansionState.EXPANDED;
                }
                else
                {
                    node.agentBExpansion = CbsNode.ExpansionState.EXPANDED;
                }

                var     newConstraint = new CbsConstraint(conflict, instance, doLeftChild);
                CbsNode child         = new CbsNode(node, newConstraint, conflictingAgentIndex);

                if (closedList.ContainsKey(child) == false)
                {
                    int minCost = -1;
                    //if (this.useMddHeuristic)
                    //    minCost = node.GetGroupCost(conflictingAgentIndex) + 1;
                    bool success = child.Replan(conflictingAgentIndex, this.minDepth, null, -1, minCost); // The node takes the max between minDepth and the max time over all constraints.

                    if (success == false)
                    {
                        return(null); // A timeout probably occured
                    }
                    if (debug)
                    {
                        Debug.WriteLine("Child hash: " + child.GetHashCode());
                        Debug.WriteLine("Child cost: " + child.totalCost);
                        Debug.WriteLine("Child min ops to solve: " + child.minOpsToSolve);
                        Debug.WriteLine("Child num of agents that conflict: " + child.totalInternalAgentsThatConflict);
                        Debug.WriteLine("Child num of internal conflicts: " + child.totalConflictsBetweenInternalAgents);
                        Debug.WriteLine("");
                    }

                    if (child.totalCost < node.totalCost && groupSize == 1) // Catch the error early
                    {
                        child.Print();
                        Debug.WriteLine("Child plan: (cost {0})", child.allSingleAgentCosts[conflictingAgentIndex]);
                        child.allSingleAgentPlans[conflictingAgentIndex].PrintPlan();
                        Debug.WriteLine("Parent plan: (cost {0})", node.allSingleAgentCosts[conflictingAgentIndex]);
                        node.allSingleAgentPlans[conflictingAgentIndex].PrintPlan();
                        Debug.Assert(false, "Single agent node with lower cost than parent! " + child.totalCost + " < " + node.totalCost);
                    }

                    return(child);
                }
                else
                {
                    this.closedListHits++;
                    closedListHitChildCost = this.closedList[child].totalCost;
                    if (debug)
                    {
                        Debug.WriteLine("Child already in closed list!");
                    }
                }
            }
            else
            {
                if (debug)
                {
                    Debug.WriteLine("Child already generated before");
                }
            }

            return(null);
        }
Exemplo n.º 14
0
        public virtual void Expand(CbsNode node)
        {
            CbsConflict conflict = node.GetConflict();
            CbsNode     child;

            if (this.mergeThreshold != -1 && ShouldMerge(node))
            {
                child = new CbsNode(node, node.agentsGroupAssignment[conflict.agentA], node.agentsGroupAssignment[conflict.agentB]);
                if (closedList.ContainsKey(child) == false) // We may have already merged these agents in the parent
                {
                    if (debug)
                    {
                        Debug.WriteLine("Merging agents {0} and {1}", conflict.agentA, conflict.agentB);
                    }
                    closedList.Add(child, child);
                    bool success = child.Replan(conflict.agentA, this.minDepth); // or agentB. Doesn't matter - they're in the same group.
                    if (debug)
                    {
                        Debug.WriteLine("New cost: " + child.totalCost);
                        var constraints = child.GetConstraints();
                        Debug.WriteLine(constraints.Count + " Remaining internal constraints:");
                        foreach (CbsConstraint constraint in constraints)
                        {
                            Debug.WriteLine(constraint);
                        }
                        var externalConstraints = (HashSet_U <CbsConstraint>) this.instance.parameters[CBS_LocalConflicts.CONSTRAINTS];
                        Debug.WriteLine(externalConstraints.Count.ToString() + " external constraints: ");
                        foreach (CbsConstraint constraint in externalConstraints)
                        {
                            Debug.WriteLine(constraint);
                        }
                        Debug.WriteLine("New conflict: " + child.GetConflict());
                        Debug.Write("New agent group assignments: ");
                        for (int i = 0; i < child.agentsGroupAssignment.Length; i++)
                        {
                            Debug.Write(" " + child.agentsGroupAssignment[i]);
                        }
                        Debug.WriteLine("");
                        Debug.Write("Single agent costs: ");
                        for (int i = 0; i < child.allSingleAgentCosts.Length; i++)
                        {
                            Debug.Write(" " + child.allSingleAgentCosts[i]);
                        }
                        Debug.WriteLine("");
                        child.CalculateJointPlan().PrintPlan();
                        Debug.WriteLine("");
                        Debug.WriteLine("");
                    }
                    this.maxSizeGroup = Math.Max(this.maxSizeGroup, child.replanSize);

                    if (success == false) // A timeout probably occured
                    {
                        return;
                    }

                    if (node.totalCost <= maxCost) // FIXME: Code dup with other new node creations
                    {
                        openList.Add(child);
                        this.highLevelGenerated++;
                        this.addToGlobalConflictCount(child.GetConflict());
                    }
                }
                else
                {
                    closedListHits++;
                }
                return;
            }

            // Expand node, possibly partially:
            // Generate left child:
            int agentAGroupSize = node.GetGroupSize(node.agentsGroupAssignment[conflict.agentA]);

            if (node.agentAExpansion == CbsNode.ExpansionState.NOT_EXPANDED && conflict.vertex == true &&
                conflict.timeStep >= node.allSingleAgentCosts[conflict.agentA] && // TODO: Consider checking directly whether at the time of the conflict the agent would be at its goal according to the node.singleAgentPlans. It may be more readable.
                agentAGroupSize == 1)
            // Conflict happens when or after agent A reaches its goal, and agent A is in a single agent group.
            // With multi-agent groups, banning the goal doesn't guarantee a higher cost solution,
            // since if an agent is forced to take a longer route it may enable another agent in the group
            // to take a shorter route, getting an alternative solution of the same cost
            // The left child would cost a lot because:
            // A) All WAIT moves in the goal before leaving it now add to the g.
            // B) We force the low level to compute a path longer than the optimal,
            //    and with a bad suprise towards the end in the form of a constraint,
            //    so the low-level's SIC heuristic performs poorly.
            // C) We're banning the GOAL from all directions (since this is a vertex conflict),
            //    so any alternative plan will at least cost 1 more.
            //    We're ignoring edge conflicts because they can only happen at the goal when reaching it,
            //    and aren't guaranteed to increase the cost because the goal can still be possibly reached from another edge.
            {
                if (debug)
                {
                    Debug.WriteLine("Skipping left child");
                }
                node.agentAExpansion = CbsNode.ExpansionState.DEFERRED;
                int agentAOldCost = node.allSingleAgentCosts[conflict.agentA];
                // Add the minimal delta in the child's cost:
                // since we're banning the goal at conflict.timeStep, it must at least do conflict.timeStep+1 steps
                node.totalCost += (ushort)(conflict.timeStep + 1 - agentAOldCost);
                openList.Add(node); // Re-insert node into open list with higher cost, don't re-increment global conflict counts
                this.partialExpansions++;
            }
            else if (node.agentAExpansion != CbsNode.ExpansionState.EXPANDED)
            // Agent A expansion already skipped in the past or not forcing A from its goal - finally generate the child:
            {
                if (debug)
                {
                    Debug.WriteLine("Generating left child");
                }
                var newConstraint = new CbsConstraint(conflict, instance, true);
                child = new CbsNode(node, newConstraint, conflict.agentA);

                if (closedList.ContainsKey(child) == false)
                {
                    closedList.Add(child, child);
                    if (child.Replan(conflict.agentA, this.minDepth)) // The node takes the max between minDepth and the max time over all constraints.
                    {
                        if (child.totalCost < node.totalCost && agentAGroupSize == 1)
                        {
                            Debug.WriteLine("");
                            Debug.Write("Single agent costs: ");
                            for (int i = 0; i < child.allSingleAgentCosts.Length; i++)
                            {
                                Debug.Write(" " + child.allSingleAgentCosts[i]);
                            }
                            Debug.WriteLine("");
                            //Debug.WriteLine("Offending plan:");
                            //child.CalculateJointPlan().PrintPlan();
                            Debug.WriteLine("Chlid plan: (cost {0})", child.allSingleAgentCosts[conflict.agentA]);
                            child.allSingleAgentPlans[conflict.agentA].PrintPlan();
                            Debug.WriteLine("Parent plan: (cost {0})", node.allSingleAgentCosts[conflict.agentA]);
                            node.allSingleAgentPlans[conflict.agentA].PrintPlan();
                            Debug.Assert(false, "Single agent node with lower cost than parent! " + child.totalCost + " < " + node.totalCost);
                        }
                        if (child.totalCost <= this.maxCost)
                        {
                            openList.Add(child);
                            this.highLevelGenerated++;
                            addToGlobalConflictCount(child.GetConflict());
                            if (debug)
                            {
                                Debug.WriteLine("Child cost: " + child.totalCost);
                                Debug.WriteLine("Child hash: " + child.GetHashCode());
                                Debug.WriteLine("");
                            }
                        }
                    }
                    else // A timeout probably occured
                    {
                        return;
                    }
                }
                else
                {
                    this.closedListHits++;
                }
                node.agentAExpansion = CbsNode.ExpansionState.EXPANDED;
            }

            // Generate right child:
            int agentBGroupSize = node.GetGroupSize(node.agentsGroupAssignment[conflict.agentB]);

            if (node.agentBExpansion == CbsNode.ExpansionState.NOT_EXPANDED && conflict.vertex == true &&
                conflict.timeStep >= node.allSingleAgentCosts[conflict.agentB] &&
                agentBGroupSize == 1) // Again, skip expansion
            {
                if (debug)
                {
                    Debug.WriteLine("Skipping right child");
                }
                if (node.agentAExpansion == CbsNode.ExpansionState.DEFERRED)
                {
                    throw new Exception("Unexpected: Expansion of both children differed, but this is a vertex conflict so that means the targets for the two agents are equal, which is illegal");
                }

                node.agentBExpansion = CbsNode.ExpansionState.DEFERRED;
                int agentBOldCost = node.allSingleAgentCosts[conflict.agentB];
                node.totalCost += (ushort)(conflict.timeStep + 1 - agentBOldCost);
                openList.Add(node); // Re-insert node into open list with higher cost, don't re-increment global conflict counts
                this.partialExpansions++;
                // TODO: Code duplication with agentA. Make this into a function.
            }
            else if (node.agentBExpansion != CbsNode.ExpansionState.EXPANDED)
            {
                if (debug)
                {
                    Debug.WriteLine("Generating right child");
                }
                var newConstraint = new CbsConstraint(conflict, instance, false);
                child = new CbsNode(node, newConstraint, conflict.agentB);

                if (closedList.ContainsKey(child) == false)
                {
                    closedList.Add(child, child);
                    if (child.Replan(conflict.agentB, this.minDepth)) // The node takes the max between minDepth and the max time over all constraints.
                    {
                        if (child.totalCost < node.totalCost && node.agentAExpansion == CbsNode.ExpansionState.EXPANDED &&
                            agentBGroupSize == 1)
                        {
                            Debug.WriteLine("");
                            Debug.Write("Single agent costs: ");
                            for (int i = 0; i < child.allSingleAgentCosts.Length; i++)
                            {
                                Debug.Write(" " + child.allSingleAgentCosts[i]);
                            }
                            Debug.WriteLine("");
                            //Debug.WriteLine("Offending plan:");
                            //child.CalculateJointPlan().PrintPlan();
                            Debug.WriteLine("Chlid plan: (cost {0})", child.allSingleAgentCosts[conflict.agentB]);
                            child.allSingleAgentPlans[conflict.agentB].PrintPlan();
                            Debug.WriteLine("Parent plan: (cost {0})", node.allSingleAgentCosts[conflict.agentB]);
                            node.allSingleAgentPlans[conflict.agentB].PrintPlan();
                            Debug.Assert(false, "Single agent node with lower cost than parent! " + child.totalCost + " < " + node.totalCost);
                        }
                        if (child.totalCost <= this.maxCost)
                        {
                            openList.Add(child);
                            this.highLevelGenerated++;
                            addToGlobalConflictCount(child.GetConflict());
                            if (debug)
                            {
                                Debug.WriteLine("Child cost: " + child.totalCost);
                                Debug.WriteLine("Child hash: " + child.GetHashCode());
                                Debug.WriteLine("");
                            }
                        }
                    }
                    else // A timeout probably occured
                    {
                        return;
                    }
                }
                else
                {
                    this.closedListHits++;
                }
                node.agentBExpansion = CbsNode.ExpansionState.EXPANDED;
            }
        }
Exemplo n.º 15
0
 protected override bool ShouldMerge(CbsNode node)
 {
     return(node.ShouldMerge(mergeThreshold, globalConflictsCounter));
 }
Exemplo n.º 16
0
        public bool Solve()
        {
            //this.SetGlobals(); // Again, because we might be resuming a search that was stopped.

            int initialEstimate = 0;

            if (openList.Count > 0)
            {
                initialEstimate = ((CbsNode)openList.Peek()).totalCost;
            }

            int maxExpandedNodeCostPlusH = -1;
            int currentCost = -1;

            while (openList.Count > 0)
            {
                // Check if max time has been exceeded
                if (runner.ElapsedMilliseconds() > Constants.MAX_TIME)
                {
                    this.totalCost = Constants.TIMEOUT_COST;
                    Console.WriteLine("Out of time");
                    this.solutionDepth = ((CbsNode)openList.Peek()).totalCost - initialEstimate; // A minimum estimate
                    this.Clear();                                                                // Total search time exceeded - we're not going to resume this search.
                    this.CleanGlobals();
                    return(false);
                }

                var currentNode = (CbsNode)openList.Remove();
                currentNode.ChooseConflict();

                // A cardinal conflict may have been found, increasing the h of the node.
                // Check if the node needs to be pushed back into the open list.
                if (this.openList.Count != 0 &&
                    currentNode.f > ((CbsNode)this.openList.Peek()).f)
                {
                    if (this.debug)
                    {
                        Debug.Print("Pushing back the node into the open list with an increased h.");
                    }
                    this.openList.Add(currentNode);
                    this.nodesPushedBack++;
                    continue;
                    // Notice that even though we may iterate over conflicts later,
                    // even there is a conflict that we can identify as cardinal,
                    // then the first conflict chosen _will_ be cardinal, so this is
                    // the only place we need allow pushing nodes back.
                    // We may discover cardinal conflicts in hindsight later, but there
                    // would be no point in pushing their node back at that point,
                    // as we would've already made the split by then.
                }

                this.addToGlobalConflictCount(currentNode.GetConflict()); // TODO: Make CBS_GlobalConflicts use nodes that do this automatically after choosing a conflict

                if (debug)
                {
                    currentNode.Print();
                }

                if (currentNode.totalCost > currentCost) // Needs to be here because the goal may have a cost unseen before
                {
                    currentCost = currentNode.totalCost;
                    this.nodesExpandedWithGoalCost = 0;
                }
                else if (currentNode.totalCost == currentCost) // check needed because macbs node cost isn't exactly monotonous
                {
                    this.nodesExpandedWithGoalCost++;
                }

                // Check if node is the goal
                if (currentNode.GoalTest())
                {
                    //Debug.Assert(currentNode.totalCost >= maxExpandedNodeCostPlusH, "CBS goal node found with lower cost than the max cost node ever expanded: " + currentNode.totalCost + " < " + maxExpandedNodeCostPlusH);
                    // This is subtle, but MA-CBS may expand nodes in a non non-decreasing order:
                    // If a node with a non-optimal constraint is expanded and we decide to merge the agents,
                    // the resulting node can have a lower cost than before, since we ignore the non-optimal constraint
                    // because the conflict it addresses is between merged nodes.
                    // The resulting lower-cost node will have other constraints, that will raise the cost of its children back to at least its original cost,
                    // since the node with the non-optimal constraint was only expanded because its competitors that had an optimal
                    // constraint to deal with the same conflict apparently found the other conflict that I promise will be found,
                    // and so their cost was not smaller than this sub-optimal node.
                    // To make MA-CBS costs non-decreasing, we can choose not to ignore constraints that deal with conflicts between merged nodes.
                    // That way, the sub-optimal node will find a sub-optimal merged solution and get a high cost that will push it deep into the open list.
                    // But the cost would be to create a possibly sub-optimal merged solution where an optimal solution could be found instead, and faster,
                    // since constraints make the low-level heuristic perform worse.
                    // For an example for this subtle case happening, see problem instance 63 of the random grid with 4 agents,
                    // 55 grid cells and 9 obstacles.

                    if (debug)
                    {
                        Debug.WriteLine("-----------------");
                    }
                    this.totalCost     = currentNode.totalCost;
                    this.solution      = currentNode.CalculateJointPlan();
                    this.solutionDepth = this.totalCost - initialEstimate;
                    this.goalNode      = currentNode; // Saves the single agent plans and costs
                    // The joint plan is calculated on demand.
                    this.Clear();                     // Goal found - we're not going to resume this search
                    this.CleanGlobals();
                    return(true);
                }

                if (currentNode.totalCost >= this.targetCost || // Node is good enough
                    //(this.targetCost != int.MaxValue &&
                    //this.lowLevelGenerated > Math.Pow(Constants.NUM_ALLOWED_DIRECTIONS, this.instance.m_vAgents.Length))
                    this.solver.GetAccumulatedGenerated() > this.lowLevelGeneratedCap || // Stop because this is taking too long.
                                                                                         // We're looking at _generated_ low level nodes since that's an indication to the amount of work done,
                                                                                         // while expanded nodes is an indication of the amount of good work done.
                                                                                         // b**k is the maximum amount of nodes we'll generate if we expand this node with A*.
                    (this.milliCap != int.MaxValue &&                                    // (This check is much cheaper than the method call)
                     this.runner.ElapsedMilliseconds() > this.milliCap))                 // Search is taking too long.
                {
                    if (debug)
                    {
                        Debug.WriteLine("-----------------");
                    }
                    this.totalCost = maxExpandedNodeCostPlusH; // This is the min possible cost so far.
                    this.openList.Add(currentNode);            // To be able to continue the search later
                    this.CleanGlobals();
                    return(false);
                }

                if (maxExpandedNodeCostPlusH < currentNode.totalCost + currentNode.h)
                {
                    maxExpandedNodeCostPlusH = currentNode.totalCost + currentNode.h;
                    if (debug)
                    {
                        Debug.Print("New max F: {0}", maxExpandedNodeCostPlusH);
                    }
                }

                // Expand
                bool wasUnexpandedNode = (currentNode.agentAExpansion == CbsNode.ExpansionState.NOT_EXPANDED &&
                                          currentNode.agentBExpansion == CbsNode.ExpansionState.NOT_EXPANDED);
                Expand(currentNode);
                if (wasUnexpandedNode)
                {
                    highLevelExpanded++;
                }
                // Consider moving the following into Expand()
                if (currentNode.agentAExpansion == CbsNode.ExpansionState.EXPANDED &&
                    currentNode.agentBExpansion == CbsNode.ExpansionState.EXPANDED) // Fully expanded
                {
                    currentNode.Clear();
                }
            }

            this.totalCost = Constants.NO_SOLUTION_COST;
            this.Clear(); // unsolvable problem - we're not going to resume it
            this.CleanGlobals();
            return(false);
        }
Exemplo n.º 17
0
        /// <summary>
        ///
        /// </summary>
        /// <param name="node"></param>
        /// <param name="children"></param>
        /// <param name="adoptBy">If not given, adoption is done by expanded node</param>
        /// <returns>true if adopted - need to rerun this method, ignoring the returned children from this call, bacause adoption was performed</returns>
        protected bool ExpandImpl(CbsNode node, out IList <CbsNode> children, out bool reinsertParent)
        {
            CbsConflict conflict = node.GetConflict();

            children = new List <CbsNode>();

            CbsNode child;

            reinsertParent = false;
            int  closedListHitChildCost;
            bool leftSameCost  = false; // To quiet the compiler
            bool rightSameCost = false;


            // Generate left child:
            child = ConstraintExpand(node, true, out closedListHitChildCost);
            if (child != null)
            {
                if (child == node) // Expansion deferred
                {
                    reinsertParent = true;
                }
                else // New child
                {
                    children.Add(child);
                    leftSameCost = child.totalCost == node.totalCost;
                }
            }
            else  // A timeout occured, or the child was already in the closed list.
            {
                if (closedListHitChildCost != -1)
                {
                    leftSameCost = closedListHitChildCost == node.totalCost;
                }
            }

            if (runner.ElapsedMilliseconds() > Constants.MAX_TIME)
            {
                return(false);
            }

            // Generate right child:
            child = ConstraintExpand(node, false, out closedListHitChildCost);
            if (child != null)
            {
                if (child == node) // Expansion deferred
                {
                    reinsertParent = true;
                }
                else // New child
                {
                    children.Add(child);
                    rightSameCost = child.totalCost == node.totalCost;
                }
            }
            else  // A timeout occured, or the child was already in the closed list.
            {
                if (closedListHitChildCost != -1)
                {
                    rightSameCost = closedListHitChildCost == node.totalCost;
                }
            }


            return(false);
        }
Exemplo n.º 18
0
 protected bool MergeConflicting(CbsNode node)
 {
     return(node.ShouldMerge(mergeThreshold, globalConflictsCounter));
 }
Exemplo n.º 19
0
 protected virtual bool ShouldMerge(CbsNode node)
 {
     return(node.ShouldMerge(mergeThreshold));
 }
Exemplo n.º 20
0
        public virtual bool Expand(CbsNode node, CbsConflict conflict)
        {
            if (runner.ElapsedMilliseconds() > Constants.MAX_TIME)
            {
                return(false);
            }
            highLevelExpanded++;
            if (conflict == null)
            {
                this.totalCost = node.totalCost;
                this.goalNode  = node;
                this.solution  = node.CalculateJointPlan();
                this.Clear();
                return(true);
            }
            CbsNode       toAdd;
            CbsConstraint con2       = new CbsConstraint(conflict, instance, false);
            CbsConstraint con1       = new CbsConstraint(conflict, instance, true);
            byte          stepLength = 0;

            if (conflict.vertex)
            {
                stepLength = 1;
            }
            bool ok1 = false, ok2 = false;

            if (node.totalCost + conflict.timeStep + stepLength - node.PathLength(conflict.agentA) <= fBound)
            {
                ok1 = true;
                if (node.DoesMustConstraintAllow(con1))
                {
                    toAdd = new CbsNode(node, con1, conflict.agentA);
                    toAdd.SetMustConstraint(con2);

                    if (toAdd.Replan3b(conflict.agentA, Math.Max(minCost, conflict.timeStep)))
                    {
                        this.highLevelGenerated++;
                        if (toAdd.totalCost <= fBound)
                        {
                            if (Expand(toAdd, toAdd.GetConflict()))
                            {
                                return(true);
                            }
                        }
                        else if (toAdd.totalCost < nextF)
                        {
                            nextF = toAdd.totalCost;
                        }
                    }
                }
            }

            if (node.totalCost + conflict.timeStep + stepLength - node.PathLength(conflict.agentB) <= fBound)
            {
                ok2 = true;
                if (node.DoesMustConstraintAllow(con2))
                {
                    toAdd = new CbsNode(node, con2, conflict.agentB);
                    toAdd.SetMustConstraint(con1);

                    if (toAdd.Replan3b(conflict.agentB, Math.Max(minCost, conflict.timeStep)))
                    {
                        this.highLevelGenerated++;
                        if (toAdd.totalCost <= fBound)
                        {
                            if (Expand(toAdd, toAdd.GetConflict()))
                            {
                                return(true);
                            }
                        }
                        else if (toAdd.totalCost < nextF)
                        {
                            nextF = toAdd.totalCost;
                        }
                    }
                }
            }

            if (ok1 && ok2)
            {
                toAdd = new CbsNode(node, con1, conflict.agentA);
                if (toAdd.Replan3b(conflict.agentA, Math.Max(minCost, conflict.timeStep)))
                {
                    if (toAdd.totalCost <= fBound)
                    {
                        toAdd = new CbsNode(toAdd, con2, conflict.agentB);
                        if (toAdd.Replan(conflict.agentB, Math.Max(minCost, conflict.timeStep)))
                        // FIXME: Should this really use the regular Replan() or was this a typo?
                        {
                            this.highLevelGenerated++;
                            if (toAdd.totalCost <= fBound)
                            {
                                if (Expand(toAdd, toAdd.GetConflict()))
                                {
                                    return(true);
                                }
                            }
                            else if (toAdd.totalCost < nextF)
                            {
                                nextF = toAdd.totalCost;
                            }
                        }
                    }
                }
            }
            return(false);
        }
Exemplo n.º 21
0
        public bool Solve()
        {
            this.SetGlobals(); // Again, because we might be resuming a search that was stopped.

            //Debug.WriteLine("Solving Sub-problem On Level - " + mergeThreshold);
            int initialEstimate = 0;

            if (openList.Count > 0)
            {
                initialEstimate = ((CbsNode)openList.Peek()).totalCost;
            }

            int maxExpandedCost = -1;

            while (openList.Count > 0)
            {
                // Check if max time has been exceeded
                if (runner.ElapsedMilliseconds() > Constants.MAX_TIME)
                {
                    totalCost = Constants.TIMEOUT_COST;
                    Console.WriteLine("Out of time");
                    this.Clear(); // Total search time exceeded - we're not going to resume this search.
                    this.CleanGlobals();
                    return(false);
                }
                var currentNode = (CbsNode)openList.Remove();

                if (debug)
                {
                    Debug.WriteLine("");
                    Debug.WriteLine("");
                    Debug.WriteLine("Node hash: " + currentNode.GetHashCode());
                    Debug.WriteLine("Total cost so far: " + currentNode.totalCost);
                    Debug.WriteLine("Expansion state: " + currentNode.agentAExpansion + ", " + currentNode.agentBExpansion);
                    Debug.WriteLine("Node depth: " + currentNode.depth);
                    var constraints = currentNode.GetConstraints();
                    Debug.WriteLine(constraints.Count.ToString() + " internal constraints so far: ");
                    foreach (CbsConstraint constraint in constraints)
                    {
                        Debug.WriteLine(constraint);
                    }
                    var externalConstraints = (HashSet_U <CbsConstraint>) this.instance.parameters[CBS_LocalConflicts.CONSTRAINTS];
                    Debug.WriteLine(externalConstraints.Count.ToString() + " external constraints: ");
                    foreach (CbsConstraint constraint in externalConstraints)
                    {
                        Debug.WriteLine(constraint);
                    }
                    Debug.WriteLine("Conflict: " + currentNode.GetConflict());
                    Debug.Write("Agent group assignments: ");
                    for (int i = 0; i < currentNode.agentsGroupAssignment.Length; i++)
                    {
                        Debug.Write(" " + currentNode.agentsGroupAssignment[i]);
                    }
                    Debug.WriteLine("");
                    Debug.Write("Single agent costs: ");
                    for (int i = 0; i < currentNode.allSingleAgentCosts.Length; i++)
                    {
                        Debug.Write(" " + currentNode.allSingleAgentCosts[i]);
                    }
                    Debug.WriteLine("");
                    currentNode.CalculateJointPlan().PrintPlan();
                    Debug.WriteLine("");
                    Debug.WriteLine("");
                }

                maxExpandedCost = Math.Max(maxExpandedCost, currentNode.totalCost);

                // Check if node is the goal
                if (currentNode.GoalTest())
                {
                    Debug.Assert(currentNode.totalCost >= maxExpandedCost, "CBS goal node found with lower cost than the max cost node ever expanded: " + currentNode.totalCost + " < " + maxExpandedCost);
                    // This is subtle, but MA-CBS may expand nodes in a non non-decreasing order:
                    // If a node with a non-optimal constraint is expanded and we decide to merge the agents,
                    // the resulting node can have a lower cost than before, since we ignore the non-optimal constraint
                    // because the conflict it addresses is between merged nodes.
                    // The resulting lower-cost node will have other constraints, that will raise the cost of its children back to at least its original cost,
                    // since the node with the non-optimal constraint was only expanded because its competitors that had an optimal
                    // constraint to deal with the same conflict apparently found the other conflict that I promise will be found,
                    // and so their cost was not smaller than this sub-optimal node.
                    // To make MA-CBS costs non-decreasing, we can choose not to ignore constraints that deal with conflicts between merged nodes.
                    // That way, the sub-optimal node will find a sub-optimal merged solution and get a high cost that will push it deep into the open list.
                    // But the cost would be to create a possibly sub-optimal merged solution where an optimal solution could be found instead, and faster,
                    // since constraints make the low-level heuristic perform worse.
                    // For an example for this subtle case happening, see problem instance 63 of the random grid with 4 agents,
                    // 55 grid cells and 9 obstacles.

                    if (debug)
                    {
                        Debug.WriteLine("-------------------------");
                    }
                    this.totalCost     = currentNode.totalCost;
                    this.solutionDepth = this.totalCost - initialEstimate;
                    this.goalNode      = currentNode; // Saves the single agent plans and costs
                    // The joint plan is calculated on demand.
                    this.Clear();                     // Goal found - we're not going to resume this search
                    this.CleanGlobals();
                    return(true);
                }

                if (currentNode.totalCost >= this.targetCost || // Node is good enough
                    //(this.targetCost != int.MaxValue &&
                    //this.lowLevelGenerated > Math.Pow(Constants.NUM_ALLOWED_DIRECTIONS, this.instance.m_vAgents.Length))
                    this.solver.GetAccumulatedGenerated() > this.lowLevelGeneratedCap || // Stop because this is taking too long.
                                                                                         // We're looking at _generated_ low level nodes since that's an indication to the amount of work done,
                                                                                         // while expanded nodes is an indication of the amount of good work done.
                                                                                         // b**k is the maximum amount of nodes we'll generate if we expand this node with A*.
                    (this.milliCap != int.MaxValue &&                                    // (This check is much cheaper than the method call)
                     this.runner.ElapsedMilliseconds() > this.milliCap))                 // Search is taking too long.
                {
                    if (debug)
                    {
                        Debug.WriteLine("-------------------------");
                    }
                    this.totalCost = maxExpandedCost; // This is the min possible cost so far.
                    this.openList.Add(currentNode);   // To be able to continue the search later
                    this.CleanGlobals();
                    return(false);
                }

                // Expand
                bool wasUnexpandedNode = (currentNode.agentAExpansion == CbsNode.ExpansionState.NOT_EXPANDED &&
                                          currentNode.agentBExpansion == CbsNode.ExpansionState.NOT_EXPANDED);
                Expand(currentNode);
                if (wasUnexpandedNode)
                {
                    highLevelExpanded++;
                }
                // Consider moving the following into Expand()
                if (currentNode.agentAExpansion == CbsNode.ExpansionState.EXPANDED &&
                    currentNode.agentBExpansion == CbsNode.ExpansionState.EXPANDED) // Fully expanded
                {
                    currentNode.Clear();
                }
            }

            this.totalCost = Constants.NO_SOLUTION_COST;
            this.Clear(); // unsolvable problem - we're not going to resume it
            this.CleanGlobals();
            return(false);
        }