/// <summary>
/// Find the path for all the agents.
/// </summary>
/// <param name="currentTime"></param>
/// <param name="agents">agents</param>
/// <param name="obstacleNodes">The way points of the obstacles.</param>
/// <param name="lockedNodes"></param>
/// <param name="nextReoptimization">The next re-optimization time.</param>
/// <exception cref="System.NotImplementedException"></exception>
public override void FindPaths(double currentTime, List <Elements.Agent> agents)
{
Stopwatch.Restart();
//Reservation Table
_reservationTable.Clear();
//get fixed Blockage
var fixedBlockage = AgentInfoExtractor.getStartBlockage(agents, currentTime);
foreach (var agent in agents)
{
_reservationTable.Add(fixedBlockage[agent], agent.ID, MaxPriorities + 1);
}
//initial agent times
var agentTimesHeap = new FibonacciHeap <double, Agent>(HeapDirection.Increasing);
var agentTimesDict = agents.Where(a => !a.FixedPosition).ToDictionary(a => a, a => agentTimesHeap.Enqueue(a.ArrivalTimeAtNextNode, a));
//initial agent nodes
var agentPrios = agents.Where(a => !a.FixedPosition).ToDictionary(a => a.ID, a => 0);
var agentNodes = agents.Where(a => !a.FixedPosition).ToDictionary(a => a.ID, a => 0);
var agentReservations = agents.Where(a => !a.FixedPosition).ToDictionary(a => a.ID, a => new List <ReservationTable.Interval>());
//initiate action generator
var actionGenerator = new Dictionary <int, PASActionGenerator>();
foreach (var agent in agents.Where(a => !a.FixedPosition))
{
//Create RRA* search if necessary.
//Necessary if the agent has none or the agents destination has changed
ReverseResumableAStar rraStar;
if (!rraStars.TryGetValue(agent.ID, out rraStar) || rraStar.StartNode != agent.DestinationNode)
{
rraStars[agent.ID] = new ReverseResumableAStar(Graph, agent, agent.Physics, agent.DestinationNode);
}
actionGenerator.Add(agent.ID, new PASActionGenerator(Graph, LengthOfAWaitStep, agent, rraStars[agent.ID]));
}
//action sorter
var actionSorter = new FibonacciHeap <double, Tuple <int, List <ReservationTable.Interval>, List <Collision> > >(HeapDirection.Increasing);
//loop
double cancelAt = currentTime + LengthOfAWindow;
while (agentTimesHeap.Count > 0 && Stopwatch.ElapsedMilliseconds / 1000.0 < RuntimeLimitPerAgent * agents.Count * 0.9 && Stopwatch.ElapsedMilliseconds / 1000.0 < RunTimeLimitOverall)
{
//pick the agent that has the smallest time
if (cancelAt <= agentTimesHeap.Top.Priority)
{
break;
}
var currentAgent = agentTimesHeap.Top.Value;
var currentActionGenerator = actionGenerator[currentAgent.ID];
var currentAgentNode = agentNodes[currentAgent.ID];
var currentAgentReservations = agentReservations[currentAgent.ID];
//if (currentAgent.ID == 41)
// currentAgent.ID = currentAgent.ID;
var reservationSuccessfull = false;
//initiate sorter
while (actionSorter.Count > 0)
{
actionSorter.Dequeue();
}
//get and sort actions
var actions = currentActionGenerator.GetActions(currentAgentNode, agentPrios[currentAgent.ID], _reservationTable);
var allInfinity = actions.All(a => double.IsInfinity(currentActionGenerator.h(currentAgentNode, a.Item1))) && actions.Count > 1;
foreach (var action in actions)
{
actionSorter.Enqueue(currentActionGenerator.g(action.Item1) + currentActionGenerator.h(currentAgentNode, action.Item1, allInfinity), action);
}
//try to reserve
while (actionSorter.Count > 0)
{
var actionNode = actionSorter.Top.Value.Item1;
var actionReservatios = actionSorter.Top.Value.Item2;
var actionCollisions = actionSorter.Top.Value.Item3;
actionSorter.Dequeue();
//reservation possible?
if (actionCollisions == null || actionCollisions.All(c => c.priority < agentPrios[currentAgent.ID] || c.agentId == currentAgent.ID))
{
//delete other reservations
if (actionCollisions != null)
{
//delete own reservations till last turn node
while (currentAgentReservations.Count > 0 &&
currentAgentReservations.Last().Start >= currentActionGenerator.NodeTime[currentActionGenerator.NodeBackpointerLastStopId[actionNode]] - ReservationTable.TOLERANCE)
{
_reservationTable.Remove(currentAgentReservations.Last());
currentAgentReservations.RemoveAt(currentAgentReservations.Count - 1);
}
//delete other reservations
foreach (var collsion in actionCollisions.Where(c => c.agentId != currentAgent.ID))
{
var nodeToSetBackToPast = agentNodes[collsion.agentId];
var reservationToSetBackToPast = agentReservations[collsion.agentId];
setAgentBackToPast(collsion.agentId, actionGenerator[collsion.agentId], ref nodeToSetBackToPast, ref reservationToSetBackToPast, collsion.time);
agentNodes[collsion.agentId] = nodeToSetBackToPast;
agentReservations[collsion.agentId] = reservationToSetBackToPast; //note: I know - it is only a reference to the list => but for clearance
}
}
//reserve me
_reservationTable.Add(actionReservatios, currentAgent.ID, agentPrios[currentAgent.ID]);
currentAgentReservations.AddRange(actionReservatios);
//set my node
currentAgentNode = agentNodes[currentAgent.ID] = actionNode;
//reached destination?
if (currentActionGenerator.NodeTo2D(currentAgentNode) == currentAgent.DestinationNode)
{
//Here the reason of commenting: Only 2 or 3 Nodes will be checked by reservation table, the rest will be added blind. If there are 10 Nodes in the hop, 7 reservations will possibly rejected. So the whole transfer will be rejected.
//cancelAt = Math.Min(cancelAt,currentActionGenerator.NodeTime[currentAgentNode]);
agentTimesHeap.Dequeue();
if (_reservationTable.IntersectionFree(currentActionGenerator.NodeTo2D(currentAgentNode), currentActionGenerator.NodeTime[currentAgentNode], double.PositiveInfinity))
{
_reservationTable.Add(currentActionGenerator.NodeTo2D(currentAgentNode), currentActionGenerator.NodeTime[currentAgentNode], double.PositiveInfinity, currentAgent.ID, MaxPriorities + 1);
}
}
else
{
//set the time and node
agentTimesHeap.ChangeKey(agentTimesDict[currentAgent], actionReservatios.Last().End);
}
//reservation successful
reservationSuccessfull = true;
break;
}
}
//could not find an action
if (reservationSuccessfull)
{
agentPrios[currentAgent.ID] = 0;
}
else
{
agentPrios[currentAgent.ID]++;
//wait step
var waitNode = currentActionGenerator.GenerateWaitNode(currentAgentNode, _reservationTable);
if (agentPrios[currentAgent.ID] < MaxPriorities)
{
if (waitNode.Item3 == null || waitNode.Item3.All(c => c.priority < agentPrios[currentAgent.ID]))
{
//delete other reservations
if (waitNode.Item3 != null)
{
foreach (var collsion in waitNode.Item3)
{
//reset agent moves
var nodeToSetBackToPast = agentNodes[collsion.agentId];
var reservationToSetBackToPast = agentReservations[collsion.agentId];
setAgentBackToPast(collsion.agentId, actionGenerator[collsion.agentId], ref nodeToSetBackToPast, ref reservationToSetBackToPast, collsion.time);
agentNodes[collsion.agentId] = nodeToSetBackToPast;
agentReservations[collsion.agentId] = reservationToSetBackToPast; //note: I know - it is only a reference to the list => but for clearance
}
}
//reserve me
currentAgentReservations.AddRange(waitNode.Item2);
_reservationTable.Add(waitNode.Item2, currentAgent.ID, agentPrios[currentAgent.ID]);
//set next node
agentTimesHeap.ChangeKey(agentTimesDict[currentAgent], waitNode.Item2.Last().End);
currentAgentNode = agentNodes[currentAgent.ID] = waitNode.Item1;
}
}
else
{
//no reservation
agentPrios[currentAgent.ID] = 0;
//set next node
agentTimesHeap.ChangeKey(agentTimesDict[currentAgent], waitNode.Item2.Last().End);
currentAgentNode = agentNodes[currentAgent.ID] = waitNode.Item1;
}
}
}//agent pick loop
// Signal potential timeout
if (Stopwatch.ElapsedMilliseconds / 1000.0 > RuntimeLimitPerAgent * agents.Count * 0.9 || Stopwatch.ElapsedMilliseconds / 1000.0 > RunTimeLimitOverall)
{
Communicator.SignalTimeout();
}
foreach (var agent in agents.Where(a => !a.FixedPosition))
{
agent.Path = agent.Path ?? new Path();
//+ WHCA* Nodes
List <ReservationTable.Interval> reservations;
actionGenerator[agent.ID].GetPathAndReservations(ref agent.Path, out reservations, agentNodes[agent.ID], 0.0);
//+ RRA* Nodes
rraStars[agent.ID].addPath(agent.Path, actionGenerator[agent.ID].NodeTo2D(agentNodes[agent.ID]));
//add the next node again
if (fixedBlockage.Count > 0 && (agent.Path.Count == 0 || agent.Path.NextAction.Node != agent.NextNode || agent.Path.NextAction.StopAtNode == false))
{
agent.Path.AddFirst(agent.NextNode, true, 0);
}
//next time ready?
if (agent.Path.Count == 0)
{
rraStars[agent.ID] = null;
}
}
}
public override void FindPaths(double currentTime, List <Agent> agents)
{
Stopwatch.Restart();
var fixedBlockage = AgentInfoExtractor.getStartBlockage(agents, currentTime);
//deadlock handling
_deadlockHandler.LengthOfAWaitStep = LengthOfAWaitStep;
_deadlockHandler.MaximumWaitTime = 30;
_deadlockHandler.Update(agents, currentTime);
//initiate biased costs
_biasedCost.Clear();
foreach (var agent in agents)
{
_biasedCost.Add(agent.ID, new Dictionary <int, double>());
}
while (true)
{
//clear reservation table
_reservationTable.Clear();
//set fixed blockage
try
{
foreach (var agent in agents)
{
//all intervals from now to the moment of stop
foreach (var interval in fixedBlockage[agent])
{
_reservationTable.Add(interval, agent.ID);
}
//reservation of next node
_reservationTable.Add(agent.NextNode, agent.ArrivalTimeAtNextNode, double.PositiveInfinity, agent.ID);
}
}
catch (RAWSimO.MultiAgentPathFinding.DataStructures.DisjointIntervalTree.IntervalIntersectionException) { }
var collisionFound = false;
foreach (var agent in agents.Where(a => !a.FixedPosition && a.NextNode != a.DestinationNode))
{
if (Stopwatch.ElapsedMilliseconds / 1000.0 > RuntimeLimitPerAgent * agents.Count * 0.9 || Stopwatch.ElapsedMilliseconds / 1000.0 > RunTimeLimitOverall)
{
Communicator.SignalTimeout();
return;
}
//remove blocking next node
_reservationTable.RemoveIntersectionWithTime(agent.NextNode, agent.ArrivalTimeAtNextNode);
if (_deadlockHandler.IsInDeadlock(agent, currentTime))
{
_deadlockHandler.RandomHop(agent, _reservationTable, currentTime, true, true);
continue;
}
//Create A* search if necessary.
//Necessary if the agent has none or the agents destination has changed
var aStar = new SpaceAStar(Graph, agent.NextNode, agent.DestinationNode, agent.OrientationAtNextNode, agent.Physics, agent, _biasedCost[agent.ID]);
var found = aStar.Search();
//the search ended with no result => just wait a moment
if (!found)
{
//Can not find a path. Maybe the agent has to wait until the blocked nodes moved
waitStep(agent);
continue;
}
//get the path
List <ReservationTable.Interval> reservations;
agent.Path.Clear();
aStar.getReservationsAndPath(agent.ArrivalTimeAtNextNode, ref agent.Path, out reservations);
reservations.Last().End = double.PositiveInfinity;
//check whether the agent collides with an other agent on its way
int collidesWithAgent;
int collidesOnNode;
if (!_reservationTable.IntersectionFree(reservations, out collidesOnNode, out collidesWithAgent))
{
agent.Path.SetStopBeforeNode(collidesOnNode);
//collision => add biased cost for the current agent
//the other agent remains with the old cost. He has the higher priority
if (_biasedCost[agent.ID].ContainsKey(collidesOnNode))
{
_biasedCost[agent.ID][collidesOnNode] += BiasedCostAmount;
}
else
{
_biasedCost[agent.ID][collidesOnNode] = BiasedCostAmount;
}
collisionFound = true;
}
else
{
_reservationTable.Add(reservations, agent.ID);
}
}
if (!collisionFound)
{
return;
}
}
}
/// <summary>
/// Find the path for all the agents.
/// </summary>
/// <param name="currentTime">The current Time.</param>
/// <param name="agents">agents</param>
/// <param name="obstacleNodes">The way points of the obstacles.</param>
/// <param name="lockedNodes"></param>
/// <param name="nextReoptimization">The next re-optimization time.</param>
/// <param name="communicator">The communicator used for communication with the corresponding instance.</param>
public void FindPaths(double currentTime, List <Agent> agents, double runtimeLimit, PathPlanningCommunicator communicator)
{
if (agents.Count == 0)
{
return;
}
var stopwatch = new Stopwatch();
stopwatch.Restart();
//0: init low level solver
LowLevSolver.Init(currentTime, agents);
//1: assign each agent to a singleton group
var agentGroups = new Dictionary <int, List <Agent> >(agents.ToDictionary(a => a.ID, a => new List <Agent>(new Agent[] { a })));
var groundIdAssigments = new Dictionary <int, int>(agents.ToDictionary(a => a.ID, a => a.ID));
//2: plan a path for each group
var plannedPath = new Dictionary <int, List <PlannedPath> >(agentGroups.ToDictionary(ag => ag.Key, ag => LowLevSolver.FindPaths(ag.Value)));
var reservationTable = new ReservationTable(_graph, true, true, false);
//set fixed blockage
var fixedBlockage = AgentInfoExtractor.getStartBlockage(agents, currentTime);
foreach (var agent in fixedBlockage.Keys)
{
foreach (var interval in fixedBlockage[agent])
{
reservationTable.Add(interval, agent.ID);
}
}
//3: repeat
while (agentGroups.Count > 1)
{
reservationTable.Clear();
//4: simulate execution of all paths until a conflict occurs
int conflictAgent1 = -1;
int conflictGroup1 = -1;
int conflictAgent2 = -1;
int conflictGroup2 = -1;
int conflictNode;
var foundConflict = false;
foreach (var groupId in agentGroups.Keys)
{
for (var agentIndex = 0; agentIndex < agentGroups[groupId].Count && !foundConflict; agentIndex++)
{
foundConflict = !reservationTable.IntersectionFree(plannedPath[groupId][agentIndex].Reservations, out conflictNode, out conflictAgent1);
if (foundConflict)
{
//remember the conflict agent
conflictAgent2 = plannedPath[groupId][agentIndex].Agent.ID;
conflictGroup2 = groupId;
conflictGroup1 = groundIdAssigments[conflictAgent1];
if (conflictGroup1 == conflictGroup2)
{
foundConflict = false;
}
}
else
{
//just add intervals
reservationTable.Add(plannedPath[groupId][agentIndex].Reservations, plannedPath[groupId][agentIndex].Agent.ID);
}
}
}
if (stopwatch.ElapsedMilliseconds / 1000.0 > runtimeLimit * 0.9)
{
communicator.SignalTimeout();
break;
}
if (foundConflict)
{
//5: merge two conflicting groups into a single group
//merge
agentGroups[conflictGroup1].AddRange(agentGroups[conflictGroup2]);
//delete old group
agentGroups.Remove(conflictGroup2);
plannedPath.Remove(conflictGroup2);
//reset Assignment
for (int agentIndex = 0; agentIndex < agentGroups[conflictGroup1].Count; agentIndex++)
{
groundIdAssigments[agentGroups[conflictGroup1][agentIndex].ID] = conflictGroup1;
}
//6: cooperatively plan new group
plannedPath[conflictGroup1] = LowLevSolver.FindPaths(agentGroups[conflictGroup1]);
}
else
{
//7: until no conflicts occur
break;
}
}
//8: solution ← paths of all groups combined
foreach (var groupId in agentGroups.Keys)
{
for (var agentIndex = 0; agentIndex < agentGroups[groupId].Count; agentIndex++)
{
agentGroups[groupId][agentIndex].Path = plannedPath[groupId][agentIndex].Path;//9: return solution
if (agentGroups[groupId][agentIndex].Path.Count <= 1)
{
_deadlockHandler.RandomHop(agentGroups[groupId][agentIndex]);
}
}
}
}
/// <summary>
/// Find the path for all the agents.
/// </summary>
/// <param name="currentTime">The current time.</param>
/// <param name="agents">agents</param>
/// <param name="obstacleNodes">The way points of the obstacles.</param>
/// <param name="lockedNodes">The locked nodes.</param>
/// <param name="nextReoptimization">The next re-optimization time.</param>
/// <param name="runtimeLimit">The runtime limit.</param>
public override void FindPaths(double currentTime, List <Agent> agents)
{
Stopwatch.Restart();
//Reservation Table
_reservationTable.Clear();
var fixedBlockage = AgentInfoExtractor.getStartBlockage(agents, currentTime);
//panic times initialization
foreach (var agent in agents)
{
if (!_waitTime.ContainsKey(agent.ID))
{
_waitTime.Add(agent.ID, double.NegativeInfinity);
}
if (!_moveTime.ContainsKey(agent.ID))
{
_moveTime.Add(agent.ID, currentTime);
}
if (!_es2evadedFrom.ContainsKey(agent.ID))
{
_es2evadedFrom.Add(agent.ID, new HashSet <int>());
}
}
//set fixed blockage
try
{
foreach (var agent in agents)
{
//all intervals from now to the moment of stop
foreach (var interval in fixedBlockage[agent])
{
_reservationTable.Add(interval, agent.ID);
}
//reservation of next node
int collisionAgent;
if (_reservationTable.IntersectionFree(agent.NextNode, agent.ArrivalTimeAtNextNode, double.PositiveInfinity, out collisionAgent))
{
_reservationTable.Add(agent.NextNode, agent.ArrivalTimeAtNextNode, double.PositiveInfinity, agent.ID);
}
else
{
Debug.Assert(collisionAgent == agent.ID);
}
}
}
catch (RAWSimO.MultiAgentPathFinding.DataStructures.DisjointIntervalTree.IntervalIntersectionException) { }
//deadlock handling
if (UseDeadlockHandler)
{
_deadlockHandler.LengthOfAWaitStep = LengthOfAWaitStep;
_deadlockHandler.MaximumWaitTime = 30;
_deadlockHandler.Update(agents, currentTime);
}
//optimize Path
bool deadlockBreakingManeuver;
var reservationOwnerAgentId = -1;
int reservationOwnerNodeId = -1;
List <int> nextHopNodes;
foreach (Agent a in agents)
{
if (a.Path != null && !a.Path.IsConsistent)
{
throw new Exception("fs ex");
}
}
foreach (var agent in agents.Where(a => !a.FixedPosition && a.NextNode != a.DestinationNode && a.ArrivalTimeAtNextNode <= currentTime && a.RequestReoptimization))
{
//runtime exceeded
if (Stopwatch.ElapsedMilliseconds / 1000.0 > RuntimeLimitPerAgent * agents.Count * 0.9 || Stopwatch.ElapsedMilliseconds / 1000.0 > RunTimeLimitOverall)
{
Communicator.SignalTimeout();
return;
}
deadlockBreakingManeuver = false;
//agent is allowed to move?
if (currentTime < _waitTime[agent.ID])
{
continue;
}
//remove blocking next node
_reservationTable.RemoveIntersectionWithTime(agent.NextNode, agent.ArrivalTimeAtNextNode);
//Create RRA* search if necessary.
//Necessary if the agent has none or the agents destination has changed
ReverseResumableAStar rraStar;
if (!_rraStar.TryGetValue(agent.ID, out rraStar) || rraStar == null || rraStar.StartNode != agent.DestinationNode)
{
//new search
rraStar = new ReverseResumableAStar(Graph, agent, agent.Physics, agent.DestinationNode, new HashSet <int>());
_rraStar[agent.ID] = rraStar;
_moveTime[agent.ID] = currentTime;
}
//already found in RRA*?
var found = rraStar.Closed.Contains(agent.NextNode);
//If the search is old, the path may me blocked now
if (found && rraStar.PathContains(agent.NextNode))
{
//new search
rraStar = new ReverseResumableAStar(Graph, agent, agent.Physics, agent.DestinationNode, new HashSet <int>());
_rraStar[agent.ID] = rraStar;
found = false;
}
//Is a search processing necessary
if (!found)
{
found = rraStar.Search(agent.NextNode);
}
//the search ended with no result => just wait a moment
if (!found)
{
//new search
_rraStar[agent.ID] = null;
//still not found? Then wait!
if (_waitTime[agent.ID] < currentTime - LengthOfAWaitStep * 2f)
{
waitStep(agent, agent.ID, currentTime);
continue;
}
else
{
deadlockBreakingManeuver = true;
}
}
if (!deadlockBreakingManeuver)
{
//set the next step of the path
nextHopNodes = _getNextHopNodes(currentTime, agent, out reservationOwnerAgentId, out reservationOwnerNodeId);
//avoid going back
if (Es2BackEvadingAvoidance && nextHopNodes.Count > 1 && _es2evadedFrom[agent.ID].Contains(nextHopNodes[1]))
{
deadlockBreakingManeuver = true;
}
else if (Es2BackEvadingAvoidance)
{
_es2evadedFrom[agent.ID].Clear();
}
//found a path => set it
if (!deadlockBreakingManeuver && nextHopNodes.Count > 1)
{
_setPath(nextHopNodes, agent);
_moveTime[agent.ID] = currentTime;
continue;
}
}
//deadlock breaking maneuver due to wait time
if (!deadlockBreakingManeuver)
{
deadlockBreakingManeuver = currentTime - _moveTime[agent.ID] > MaximumWaitTime;
}
//deadlock breaking maneuver due to wait for relation circle
if (!deadlockBreakingManeuver)
{
//transitive closure of wait for relation
HashSet <int> waitForSet = new HashSet <int>();
var waitForID = agent.ID;
while (!deadlockBreakingManeuver && _waitFor.ContainsKey(waitForID))
{
if (waitForSet.Contains(waitForID))
{
deadlockBreakingManeuver = true;
}
waitForSet.Add(waitForID);
waitForID = _waitFor[waitForID];
}
}
if (!deadlockBreakingManeuver)
{
//wait a little while
waitStep(agent, reservationOwnerAgentId, currentTime);
continue;
}
else
{
//deadlock breaking maneuver must be done!
switch (_evadingStragety)
{
case EvadingStrategy.EvadeByRerouting:
//obstacle free
if (!found)
{
rraStar = _rraStar[agent.ID] = new ReverseResumableAStar(Graph, agent, agent.Physics, agent.DestinationNode, new HashSet <int>());
}
for (int tries = 1; tries <= Es1MaximumNumberOfBreakingManeuverTries; tries++)
{
//deadlock breaking maneuver must be done!
if (tries >= Es1MaximumNumberOfBreakingManeuverTries)
{
//wait a little while => for myself
waitStep(agent, agent.ID, currentTime);
break;
}
//The agent can not move
_waitFor[agent.ID] = reservationOwnerAgentId;
//add an obstacle and restart the search
rraStar.AddCustomLock(reservationOwnerNodeId);
rraStar.Clear(agent.DestinationNode, agent.NextNode);
found = rraStar.Search(agent.NextNode);
//found => get hop nodes
if (!found)
{
//wait a little while => for myself
waitStep(agent, agent.ID, currentTime);
break;
}
else
{
nextHopNodes = _getNextHopNodes(currentTime, agent, out reservationOwnerAgentId, out reservationOwnerNodeId);
if (nextHopNodes.Count > 1)
{
_setPath(nextHopNodes, agent);
break;
}
}
}
break;
case EvadingStrategy.EvadeToNextNode:
//try to find a free hop
var foundBreakingManeuverEdge = false;
var possibleEdges = new List <Edge>(Graph.Edges[agent.NextNode]);
shuffle <Edge>(possibleEdges, Randomizer);
foreach (var edge in possibleEdges.Where(e => !e.ToNodeInfo.IsLocked && (agent.CanGoThroughObstacles || !e.ToNodeInfo.IsObstacle) && !_es2evadedFrom[agent.ID].Contains(e.To)))
{
//create intervals
var intervals = _reservationTable.CreateIntervals(currentTime, currentTime, 0, agent.Physics, agent.NextNode, edge.To, true);
reservationOwnerNodeId = -1;
reservationOwnerAgentId = -1;
//check if a reservation is possible
if (_reservationTable.IntersectionFree(intervals, out reservationOwnerNodeId, out reservationOwnerAgentId))
{
foundBreakingManeuverEdge = true;
//avoid going back
if (this.Es2BackEvadingAvoidance)
{
_es2evadedFrom[agent.ID].Add(edge.To);
}
//create a path
agent.Path.Clear();
agent.Path.AddLast(edge.To, true, LengthOfAWaitStep);
break;
}
}
if (!foundBreakingManeuverEdge)
{
//Clear the nodes
_es2evadedFrom[agent.ID].Clear();
//just wait
waitStep(agent, agent.ID, currentTime);
}
break;
}
}
foreach (Agent a in agents)
{
if (a.Path != null && !a.Path.IsConsistent)
{
throw new Exception("fs ex");
}
}
//deadlock?
if (UseDeadlockHandler)
{
if (_deadlockHandler.IsInDeadlock(agent, currentTime))
{
_deadlockHandler.RandomHop(agent);
}
}
}
}
/// <summary>
/// Find the path for all the agents.
/// </summary>
/// <param name="agents">agents</param>
/// <param name="obstacleWaypoints">The way points of the obstacles.</param>
/// <param name="queues">The queues for the destination, starting with the destination point.</param>
/// <param name="nextReoptimization">The next re-optimization time.</param>
/// <returns>
/// paths
/// </returns>
/// <exception cref="System.Exception">Here I have to do something</exception>
private bool _findPaths(double currentTime, List <Agent> agents, Dictionary <int, int> agentPrios, bool lastRun, double runtimeLimit)
{
var conflictFree = true;
//Reservation Table
_reservationTable.Clear();
var fixedBlockage = AgentInfoExtractor.getStartBlockage(agents, currentTime);
SortAgents(ref agents, agentPrios);
//set fixed blockage
foreach (var interval in fixedBlockage.Values.SelectMany(d => d))
{
_reservationTable.Add(interval);
}
//deadlock handling
if (UseDeadlockHandler)
{
_deadlockHandler.LengthOfAWaitStep = LengthOfAWaitStep;
_deadlockHandler.MaximumWaitTime = 30;
_deadlockHandler.Update(agents, currentTime);
}
//optimize Path
foreach (var agent in agents.Where(a => !a.FixedPosition))
{
if (Stopwatch.ElapsedMilliseconds / 1000.0 > runtimeLimit * 0.9)
{
Communicator.SignalTimeout();
return(true);
}
//Create RRA* search if necessary.
//Necessary if the agent has none or the agents destination has changed
ReverseResumableAStar rraStar;
if (!rraStars.TryGetValue(agent.ID, out rraStar) || rraStar.StartNode != agent.DestinationNode ||
UseDeadlockHandler && _deadlockHandler.IsInDeadlock(agent, currentTime)) // TODO this last expression is used to set back the state of the RRA* in case of a deadlock - this is only a hotfix
{
rraStars[agent.ID] = new ReverseResumableAStar(Graph, agent, agent.Physics, agent.DestinationNode);
}
//search my path to the goal
var aStar = new SpaceTimeAStar(Graph, LengthOfAWaitStep, currentTime + LengthOfAWindow, _reservationTable, agent, rraStars[agent.ID]);
//the agent with a higher priority has to wait so that the others can go out of the way
aStar.WaitStepsBeforeStart = (int)(Math.Pow(2, agentPrios[agent.ID]) / 2.0);
//execute
var found = aStar.Search();
if (!found)
{
conflictFree = false;
//fall back => ignore the collisions
agentPrios[agent.ID]++;
if (!lastRun)
{
if (!AbortAtFirstConflict)
{
continue;
}
else
{
return(false);
}
}
}
//+ WHCA* Nodes
List <ReservationTable.Interval> reservations;
if (found)
{
aStar.GetPathAndReservations(ref agent.Path, out reservations);
foreach (var reservation in reservations)
{
_reservationTable.Add(reservation);
}
}
//+ RRA* Nodes
if (aStar.GoalNode >= 0)
{
rraStars[agent.ID].addPath(agent.Path, aStar.NodeTo2D(aStar.GoalNode));
}
//add the next node again
if (fixedBlockage.Count > 0 && (agent.Path.Count == 0 || agent.Path.NextAction.Node != agent.NextNode || agent.Path.NextAction.StopAtNode == false))
{
agent.Path.AddFirst(agent.NextNode, true, 0);
}
//next time ready?
if (agent.Path.Count == 0)
{
rraStars[agent.ID] = null;
}
//deadlock?
if (UseDeadlockHandler)
{
if (_deadlockHandler.IsInDeadlock(agent, currentTime))
{
_deadlockHandler.RandomHop(agent);
}
}
}
return(conflictFree);
}
