コード例 #1
0
ファイル: CFM_CBS.cs プロジェクト: Oscareps/FinalProject_OSO
        /// <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, MAM_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;
            CFMCbsNode root = new CFMCbsNode(instance.m_vAgents.Length, this); // Problem instance and various strategy data is all passed under 'this'.
            // Solve the root node - Solve with MMStar, and find conflicts
            bool solved = root.Solve();

            if (solved && root.totalCost <= this.maxCost)
            {
                this.openList.Add(root);
                this.highLevelGenerated++;
                this.closedList.Add(root, root);
            }
        }
コード例 #2
0
        public HashSet <CFMCbsConstraint> GetConstraints()
        {
            var              constraints       = new HashSet <CFMCbsConstraint>();
            CFMCbsNode       current           = this;
            CFMCbsConstraint currentConstraint = null;

            while (current.depth > 0)                                              // The root has no constraints
            {
                if (current.constraint != null &&                                  // Next check not enough if "surprise merges" happen (merges taken from adopted child)
                    current.prev.conflict != null &&                               // Can only happen for temporary lookahead nodes the were created and then later the parent adopted a goal node
                    this.agentsGroupAssignment[current.prev.conflict.agentAIndex] !=
                    this.agentsGroupAssignment[current.prev.conflict.agentBIndex]) // 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.
                // Also, merging creates a non-root node with a null constraint, and this helps avoid adding the null to the answer.

                {
                    currentConstraint = current.constraint;
                }
                TimedMove        currentMove   = current.constraint.move;
                CFMCbsConstraint newConstraint = new CFMCbsConstraint(currentConstraint.agentNum, currentMove.x, currentMove.y, currentMove.direction, currentMove.time);
                constraints.Add(newConstraint);

                current = current.prev;
            }
            return(constraints);
        }
コード例 #3
0
 public CFMCbsNode(int numberOfAgents, CFM_CBS cbs, ushort[] agentsGroupAssignment = null)
 {
     this.cbs            = cbs;
     mamPlan             = null;
     mamCost             = -1;
     allSingleAgentCosts = new int[numberOfAgents];
     countsOfInternalAgentsThatConflict = new int[numberOfAgents];
     this.nodeConflicts = null;
     if (agentsGroupAssignment == null)
     {
         this.agentsGroupAssignment = new ushort[numberOfAgents];
         for (ushort i = 0; i < numberOfAgents; i++)
         {
             this.agentsGroupAssignment[i] = i;
         }
     }
     else
     {
         this.agentsGroupAssignment = agentsGroupAssignment.ToArray <ushort>();
     }
     agentNumToIndex = new Dictionary <int, int>();
     for (int i = 0; i < numberOfAgents; i++)
     {
         agentNumToIndex[this.cbs.GetProblemInstance().m_vAgents[i].agentIndex] = i;
     }
     depth                  = 0;
     replanSize             = 1;
     agentAExpansion        = ExpansionState.NOT_EXPANDED;
     agentBExpansion        = ExpansionState.NOT_EXPANDED;
     this.prev              = null;
     this.constraint        = null;
     this.solver            = solver;
     this.singleAgentSolver = singleAgentSolver;
 }
コード例 #4
0
        public int CompareTo(IBinaryHeapItem item)
        {
            CFMCbsNode other = (CFMCbsNode)item;


            return((int)(this.mamCost - other.mamCost));
        }
コード例 #5
0
 private bool isLeftNode(CFMCbsNode node)
 {
     if (node.prev == null || node.agentToReplan == node.prev.conflict.agentAIndex)
     {
         return(true);
     }
     return(false);
 }
コード例 #6
0
        public int CompareToIgnoreH(CFMCbsNode other, bool ignorePartialExpansion = false)
        {
            // Tie breaking:

            // Prefer larger cost - higher h usually means more work needs to be done
            if (this.totalCost > other.totalCost)
            {
                return(-1);
            }
            if (this.totalCost < other.totalCost)
            {
                return(1);
            }

            // Prefer less external conflicts, even over goal nodes, as goal nodes with less external conflicts are better.
            // External conflicts are also taken into account by the low level solver to prefer less conflicts between fewer agents.
            // This only helps when this CBS is used as a low level solver, of course.
            if (this.totalConflictsWithExternalAgents < other.totalConflictsWithExternalAgents)
            {
                return(-1);
            }
            if (this.totalConflictsWithExternalAgents > other.totalConflictsWithExternalAgents)
            {
                return(1);
            }

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

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

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

            return(0);
        }
コード例 #7
0
ファイル: CFM_CBS.cs プロジェクト: Oscareps/FinalProject_OSO
        public virtual void Expand(CFMCbsNode node)
        {
            ushort             parentCost = node.totalCost;
            ushort             parentH    = node.h;
            IList <CFMCbsNode> children   = null;  // To quiet the compiler
            bool reinsertParent           = false; // To quiet the compiler


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

            foreach (var child in children)
            {
                closedList.Add(child, child);
                this.highLevelGenerated++;
                openList.Add(child);
            }
        }
コード例 #8
0
        /// <summary>
        /// Child from merge action constructor. FIXME: Code dup with previous constructor.
        /// </summary>
        /// <param name="father"></param>
        /// <param name="mergeGroupA"></param>
        /// <param name="mergeGroupB"></param>
        public CFMCbsNode(CFMCbsNode father, int mergeGroupA, int mergeGroupB)
        {
            mamPlan = null;
            mamCost = -1;
            this.allSingleAgentCosts = father.allSingleAgentCosts.ToArray <int>();
            this.countsOfInternalAgentsThatConflict = father.countsOfInternalAgentsThatConflict.ToArray <int>();
            this.nodeConflicts = null;

            this.agentsGroupAssignment = father.agentsGroupAssignment.ToArray <ushort>();
            this.agentNumToIndex       = father.agentNumToIndex;
            this.prev              = father;
            this.constraint        = null;
            this.depth             = (ushort)(this.prev.depth + 1);
            this.agentAExpansion   = ExpansionState.NOT_EXPANDED;
            this.agentBExpansion   = ExpansionState.NOT_EXPANDED;
            this.replanSize        = 1;
            this.solver            = father.solver;
            this.singleAgentSolver = father.singleAgentSolver;
            this.cbs = father.cbs;
        }
コード例 #9
0
        /// <summary>
        /// Checks the group assignment and the constraints
        /// </summary>
        /// <param name="obj"></param>
        /// <returns></returns>
        public override bool Equals(object obj)
        {
            CFMCbsNode other = (CFMCbsNode)obj;

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

            CFMCbsNode current = this;
            HashSet <CFMCbsConstraint> other_constraints = other.GetConstraints();
            HashSet <CFMCbsConstraint> constraints       = this.GetConstraints();

            foreach (CFMCbsConstraint constraint in constraints)
            {
                if (other_constraints.Contains(constraint) == false)
                {
                    return(false);
                }
                //current = current.prev;    dor comment
            }
            return(constraints.Count == other_constraints.Count);
        }
コード例 #10
0
ファイル: CFM_CBS.cs プロジェクト: Oscareps/FinalProject_OSO
        /// <summary>
        /// Create Constraints from conflict
        /// </summary>
        /// <param name="node"></param>
        /// <param name="doLeftChild"></param>
        /// <param name="closedListHitChildCost"></param>
        /// <returns></returns>
        protected CFMCbsNode ConstraintExpand(CFMCbsNode node, bool doLeftChild, out int closedListHitChildCost)
        {
            CFMCbsConflict conflict = node.GetConflict();
            int            conflictingAgentIndex = doLeftChild? conflict.agentAIndex : conflict.agentBIndex;

            CFMCbsNode.ExpansionState expansionsState           = doLeftChild ? node.agentAExpansion : node.agentBExpansion;
            CFMCbsNode.ExpansionState otherChildExpansionsState = doLeftChild ? node.agentBExpansion : node.agentAExpansion;
            string agentSide = doLeftChild? "left" : "right";
            int    groupSize = node.GetGroupSize(conflictingAgentIndex);

            closedListHitChildCost = -1;

            if (expansionsState != CFMCbsNode.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 = CFMCbsNode.ExpansionState.EXPANDED;
                }
                else
                {
                    node.agentBExpansion = CFMCbsNode.ExpansionState.EXPANDED;
                }

                var        newConstraint = new CFMCbsConstraint(conflict, instance, doLeftChild);
                CFMCbsNode child         = new CFMCbsNode(node, newConstraint, conflictingAgentIndex);

                if (closedList.ContainsKey(child) == false)
                {
                    bool success = child.Solve();

                    if (success == false)
                    {
                        return(null); // A timeout probably occured
                    }
                    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);
        }
コード例 #11
0
ファイル: CFM_CBS.cs プロジェクト: Oscareps/FinalProject_OSO
        /// <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(CFMCbsNode node, out IList <CFMCbsNode> children, out bool reinsertParent)
        {
            CFMCbsConflict conflict = node.GetConflict();

            children = new List <CFMCbsNode>();

            CFMCbsNode 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);
        }
コード例 #12
0
ファイル: CFM_CBS.cs プロジェクト: Oscareps/FinalProject_OSO
        public bool Solve()
        {
            //this.SetGlobals(); // Again, because we might be resuming a search that was stopped.

            int initialEstimate = 0;

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

            int currentCost = -1;

            Console.WriteLine("maxTime: " + Constants.MAX_TIME);
            this.startTime = this.ElapsedMillisecondsTotal();

            while (openList.Count > 0)
            {
                //Console.WriteLine(this.ElapsedMilliseconds() / 10);
                //Console.WriteLine(openList.Count);
                //Console.WriteLine(closedList.Count);

                // Check if max time has been exceeded
                if (this.ElapsedMilliseconds() > Constants.MAX_TIME)
                {
                    this.totalCost = Constants.TIMEOUT_COST;
                    Console.WriteLine("Out of time");
                    this.solutionDepth = ((CFMCbsNode)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 = (CFMCbsNode)openList.Remove();


                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     = (int)currentNode.mamCost;
                    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();
                    this.solved = true;
                    return(true);
                }

                currentNode.ChooseConflict();

                // Expand
                bool wasUnexpandedNode = (currentNode.agentAExpansion == CFMCbsNode.ExpansionState.NOT_EXPANDED &&
                                          currentNode.agentBExpansion == CFMCbsNode.ExpansionState.NOT_EXPANDED);
                Expand(currentNode);
                if (wasUnexpandedNode)
                {
                    highLevelExpanded++;
                }
                // Consider moving the following into Expand()
                if (currentNode.agentAExpansion == CFMCbsNode.ExpansionState.EXPANDED &&
                    currentNode.agentBExpansion == CFMCbsNode.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);
        }