private bool ValidatePath(ConflictTree.Node node, List <Agent> agents, out int agentId1, out int agentId2, out ReservationTable.Interval interval) { //clear _agentReservationTable.Clear(); //add next hop reservations foreach (var agent in agents.Where(a => !a.FixedPosition)) { _agentReservationTable.Add(agent.ReservationsToNextNode, agent.ID); } //get all reservations sorted var reservations = new FibonacciHeap <double, Tuple <Agent, ReservationTable.Interval> >(); foreach (var agent in agents.Where(a => !a.FixedPosition)) { foreach (var reservation in node.getReservation(agent.ID)) { reservations.Enqueue(reservation.Start, Tuple.Create(agent, reservation)); } } //check all reservations while (reservations.Count > 0) { var reservation = reservations.Dequeue().Value; int collideWithAgentId; var intersectionFree = _agentReservationTable.IntersectionFree(reservation.Item2, out collideWithAgentId); if (!intersectionFree) { agentId1 = collideWithAgentId; agentId2 = reservation.Item1.ID; interval = _agentReservationTable.GetOverlappingInterval(reservation.Item2); if (interval.End - interval.Start > ReservationTable.TOLERANCE) { return(false); } } else { _agentReservationTable.Add(reservation.Item2, reservation.Item1.ID); } } agentId1 = -1; agentId2 = -1; interval = null; return(true); }
/// <summary> /// Find the path for all the agents. /// </summary> /// <param name="currentTime">The current Time.</param> /// <param name="agents">agents</param> public override void FindPaths(double currentTime, List <Agent> agents) { Stopwatch.Restart(); //initialization data structures var conflictTree = new ConflictTree(); var Open = new FibonacciHeap <double, ConflictTree.Node>(); var solvable = true; var generatedNodes = 0; ConflictTree.Node bestNode = null; double bestTime = 0.0; //deadlock handling _deadlockHandler.LengthOfAWaitStep = LengthOfAWaitStep; _deadlockHandler.MaximumWaitTime = 30; _deadlockHandler.Update(agents, currentTime); //simply blocked foreach (var agent in agents.Where(a => a.FixedPosition)) { Graph.NodeInfo[agent.NextNode].IsLocked = true; } // TODO this only works as long as a possible solution is guaranteed - maybe instead ignore paths to plan for agents with no possible solution and hope that it clears by others moving on? //first node initialization List <Agent> unsolvableAgents = null; foreach (var agent in agents.Where(a => !a.FixedPosition)) { bool agentSolved = Solve(conflictTree.Root, currentTime, agent); if (!agentSolved) { if (unsolvableAgents == null) { unsolvableAgents = new List <Agent>() { agent } } ; else { unsolvableAgents.Add(agent); } } solvable = solvable && agentSolved; } //node selection strategy (Queue will pick the node with minimum value NodeSelectionExpression nodeObjectiveSelector = node => { switch (SearchMethod) { case CBSSearchMethod.BestFirst: return(node.SolutionCost); case CBSSearchMethod.BreathFirst: return(node.Depth); case CBSSearchMethod.DepthFirst: return((-1) * node.Depth); default: return(0); } }; //Enqueue first node if (solvable) { Open.Enqueue(conflictTree.Root.SolutionCost, conflictTree.Root); } else { Communicator.LogDefault("WARNING! Aborting CBS - could not obtain an initial solution for the following agents: " + string.Join(",", unsolvableAgents.Select(a => "Agent" + a.ID.ToString() + "(" + a.NextNode.ToString() + "->" + a.DestinationNode.ToString() + ")"))); } bestNode = conflictTree.Root; //search loop ConflictTree.Node p = conflictTree.Root; while (Open.Count > 0) { //local variables int agentId1; int agentId2; ReservationTable.Interval interval; //pop out best node p = Open.Dequeue().Value; //check the path var hasNoConflicts = ValidatePath(p, agents, out agentId1, out agentId2, out interval); //has no conflicts? if (hasNoConflicts) { bestNode = p; break; } // time up? => return the best solution if (Stopwatch.ElapsedMilliseconds / 1000.0 > RuntimeLimitPerAgent * agents.Count * 0.9 || Stopwatch.ElapsedMilliseconds / 1000.0 > RunTimeLimitOverall) { Communicator.SignalTimeout(); break; } //save best node if (bestNode == null || interval.Start > bestTime) { bestTime = interval.Start; bestNode = p; } //append child 1 var node1 = new ConflictTree.Node(agentId1, interval, p); solvable = Solve(node1, currentTime, agents.First(a => a.ID == agentId1)); if (solvable) { Open.Enqueue(node1.SolutionCost, node1); } //append child 2 var node2 = new ConflictTree.Node(agentId2, interval, p); solvable = Solve(node2, currentTime, agents.First(a => a.ID == agentId2)); if (solvable) { Open.Enqueue(node2.SolutionCost, node2); } generatedNodes += 2; } //return the solution => suboptimal foreach (var agent in agents) { agent.Path = bestNode.getSolution(agent.ID); if (_deadlockHandler.IsInDeadlock(agent, currentTime)) { _deadlockHandler.RandomHop(agent); } } }
/// <summary> /// Solves the specified node. /// </summary> /// <param name="node">The node.</param> /// <param name="currentTime">The current time.</param> /// <param name="agent">The agent.</param> /// <param name="obstacleNodes">The obstacle nodes.</param> /// <param name="lockedNodes">The locked nodes.</param> /// <returns></returns> private bool Solve(ConflictTree.Node node, double currentTime, Agent agent) { //clear reservation table _reservationTable.Clear(); //add constraints (agentId = -1 => Intervals from the tree) foreach (var constraint in node.getConstraints(agent.ID)) { _reservationTable.Add(constraint.IntervalConstraint); } //drive to next node must be possible - otherwise it is not a valid node foreach (var reservation in agent.ReservationsToNextNode) { if (!_reservationTable.IntersectionFree(reservation)) { return(false); } } //We can use WHCA Star here in a low level approach. //Window = Infinitively long var rraStar = new ReverseResumableAStar(Graph, agent, agent.Physics, agent.DestinationNode); var aStar = new SpaceTimeAStar(Graph, LengthOfAWaitStep, double.PositiveInfinity, _reservationTable, agent, rraStar); //execute var found = aStar.Search(); //+ WHCA* Nodes List <ReservationTable.Interval> reservations; Path path = new Path(); if (found) { //add all WHCA Nodes aStar.GetPathAndReservations(ref path, out reservations); #if DEBUG foreach (var reservation in reservations) { Debug.Assert(_reservationTable.IntersectionFree(reservation)); } #endif //add the next node again if (path.Count == 0 || path.NextAction.Node != agent.NextNode || path.NextAction.StopAtNode == false) { path.AddFirst(agent.NextNode, true, 0); } node.setSolution(agent.ID, path, reservations); //found return(true); } else { //not found return(false); } }