/// <summary>
/// Sets the reservations.
/// </summary>
/// <param name="agent">The agent that wants to move.</param>
/// <param name="startTime">The start time.</param>
private void setReservations(Agent agent, int node, double startTime)
{
_reservationTable.Clear();
foreach (var otherAgent in _agents)
{
//only reservations for other agents
if (otherAgent == agent)
{
continue;
}
List <ReservationTable.Interval> reservations;
Path path = null;
_nodeGenerator[otherAgent.ID].GetPathAndReservations(ref path, out reservations, _getStepNode(node, otherAgent), startTime);
if (_useFinalReservations)
{
if (reservations.Count > 0)
{
reservations.Last().End = double.PositiveInfinity;
}
else
{
reservations.Add(new ReservationTable.Interval(otherAgent.NextNode, otherAgent.ArrivalTimeAtNextNode, double.PositiveInfinity));
}
if (_reservationTable.IntersectionFree(reservations))
{
_reservationTable.Add(reservations);
}
}
else
{
//initiate this step node and time
_reservationTable.Add(reservations);
}
//reservation of the next hop
var thisStepNode = otherAgent.ReservationsToNextNode.Count - 1;
while (thisStepNode >= 0 && otherAgent.ReservationsToNextNode[thisStepNode].End > startTime)
{
if (!_useFinalReservations || _reservationTable.IntersectionFree(otherAgent.ReservationsToNextNode[thisStepNode]))
{
_reservationTable.Add(otherAgent.ReservationsToNextNode[thisStepNode]);
}
thisStepNode--;
}
}
}
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>
/// Initializes the reservation table.
/// </summary>
private void _initReservationTable()
{
_reservationTable = new ReservationTable(PathFinder.Graph, true);
//lasy initialization reservations
_reservations = new Dictionary <BotNormal, List <ReservationTable.Interval> >();
foreach (BotNormal bot in Instance.Bots)
{
if (bot.CurrentWaypoint == null)
{
bot.CurrentWaypoint = bot.Instance.WaypointGraph.GetClosestWaypoint(bot.Instance.Compound.BotCurrentTier[bot], bot.X, bot.Y);
}
_reservations.Add(bot, new List <ReservationTable.Interval>());
_reservations[bot].Add(new ReservationTable.Interval(_waypointIds[bot.CurrentWaypoint], 0, double.PositiveInfinity));
_reservationTable.Add(_reservations[bot][0]);
}
}
/// <summary>
/// Randoms the hop.
/// </summary>
/// <param name="agent">The agent.</param>
/// <param name="reservationTable">The reservation table.</param>
/// <param name="currentTime">The current time.</param>
/// <param name="finalReservation">if set to <c>true</c> [final reservation].</param>
/// <param name="insertReservation">if set to <c>true</c> [insert reservation].</param>
/// <returns></returns>
public bool RandomHop(Agent agent, ReservationTable reservationTable = null, double currentTime = 0.0, bool finalReservation = false, bool insertReservation = false)
{
//try to find a free hop
var possibleEdges = new List <Edge>(_graph.Edges[agent.NextNode]);
shuffle <Edge>(possibleEdges);
foreach (var edge in possibleEdges.Where(e => !e.ToNodeInfo.IsLocked && (agent.CanGoThroughObstacles || !e.ToNodeInfo.IsObstacle)))
{
//create intervals
if (reservationTable != null)
{
var intervals = reservationTable.CreateIntervals(currentTime, currentTime, 0, agent.Physics, agent.NextNode, edge.To, finalReservation);
//check if a reservation is possible
if (reservationTable.IntersectionFree(intervals))
{
if (insertReservation)
{
reservationTable.Add(intervals);
}
//create a path
agent.Path.Clear();
agent.Path.AddLast(edge.To, true, _rnd.NextDouble() * LengthOfAWaitStep);
return(true);
}
}
else
{
//create a path
agent.Path.Clear();
agent.Path.AddLast(edge.To, true, _rnd.NextDouble() * LengthOfAWaitStep);
return(true);
}
}
return(false);
}
/// <summary>
/// constructor
/// </summary>
/// <param name="graph">graph</param>
/// <param name="seed">The seed to use for the randomizer.</param>
/// <param name="logger">The logger to use.</param>
public WHCAnStarMethod(Graph graph, int seed, List <int> agentIds, List <int> startIds, PathPlanningCommunicator logger)
: base(graph, seed, logger)
{
if (graph.BackwardEdges == null)
{
graph.GenerateBackwardEgdes();
}
rraStars = new Dictionary <int, ReverseResumableAStar>();
_reservationTable = new ReservationTable(graph, true, false, false);
_calculatedReservations = new Dictionary <int, List <ReservationTable.Interval> >();
for (var i = 0; i < agentIds.Count; i++)
{
_calculatedReservations.Add(agentIds[i], new List <ReservationTable.Interval>(new ReservationTable.Interval[] { new ReservationTable.Interval(startIds[i], 0, double.PositiveInfinity) }));
_reservationTable.Add(_calculatedReservations[agentIds[i]]);
}
if (UseDeadlockHandler)
{
_deadlockHandler = new DeadlockHandler(graph, seed);
}
}
/// <summary>
/// Checks weather the bot can go to the next way point without collisions.
/// </summary>
/// <param name="botNormal">The bot.</param>
/// <param name="currentTime">The current time.</param>
/// <param name="waypointStart">The way point start.</param>
/// <param name="waypointEnd">The way point end.</param>
/// <param name="blockCurrentWaypointUntil">Block duration.</param>
/// <param name="rotationDuration">Rotation duration.</param>
/// <returns></returns>
public bool RegisterNextWaypoint(BotNormal botNormal, double currentTime, double blockCurrentWaypointUntil, double rotationDuration, Waypoint waypointStart, Waypoint waypointEnd)
{
//get checkpoints
var tmpReservations = _reservationTable.CreateIntervals(currentTime, blockCurrentWaypointUntil + rotationDuration, 0.0, botNormal.Physics, _waypointIds[waypointStart], _waypointIds[waypointEnd], true);
if (tmpReservations == null)
{
return(false); //no valid way point
}
//if the last node or way point is an elevator way point than add all connected nodes
if (tmpReservations.Count >= 2)
{
var lastReservation = tmpReservations[tmpReservations.Count - 1];
var elevatorWaypoint = _waypointIds[lastReservation.Node];
if (elevatorWaypoint.Elevator != null)
{
var prelastReservation = tmpReservations[tmpReservations.Count - 2];
foreach (var waypoint in elevatorWaypoint.Elevator.ConnectedPoints.Where(w => w != elevatorWaypoint))
{
tmpReservations.Add(new ReservationTable.Interval(_waypointIds[waypoint], prelastReservation.Start, prelastReservation.End));
tmpReservations.Add(new ReservationTable.Interval(_waypointIds[waypoint], lastReservation.Start, lastReservation.End));
}
}
}
//remove current
_reservationTable.Remove(_reservations[botNormal]);
//check if free
var free = _reservationTable.IntersectionFree(tmpReservations);
//check if a pod collision can occur
if (botNormal.Pod != null)
{
//checker if there is a static pod
foreach (var interval in tmpReservations)
{
if (_waypointIds[interval.Node].Pod != null)
{
//there exists a way point with a pod on it
free = false;
break;
}
}
}
if (free)
{
_reservations[botNormal] = tmpReservations;
//#RealWorldIntegration.start
if (botNormal.Instance.SettingConfig.RealWorldIntegrationCommandOutput && botNormal.Instance.SettingConfig.LogAction != null)
{
//Log the wait command
var sb = new StringBuilder();
sb.Append("#RealWorldIntegration => Bot ").Append(botNormal.ID).Append(" Drive: ");
if (blockCurrentWaypointUntil - currentTime > 0)
{
sb.Append("(wait: ").Append(blockCurrentWaypointUntil - currentTime).Append("s)");
}
for (var i = 1; i < _reservations[botNormal].Count - 1; i++)
{
sb.Append(_waypointIds[_reservations[botNormal][i].Node].ID).Append(";");
}
botNormal.Instance.SettingConfig.LogAction(sb.ToString());
// Issue the path command
Instance.RemoteController.RobotSubmitPath(
botNormal.ID, // The ID of the robot the path is send to
blockCurrentWaypointUntil - currentTime, // The time the robot shall wait before executing the path
_reservations[botNormal].Take(_reservations[botNormal].Count - 1).Select(r => _waypointIds[r.Node].ID).ToList()); // The path to execute
}
//#RealWorldIntegration.end
}
//Debug
//Log the wait command
/*
* var tmp = new StringBuilder();
* tmp.Append("Bot ").Append(botNormal.ID).Append(" Drive: ");
*
* if (blockCurrentWaypointUntil - currentTime > 0)
* tmp.Append("(wait: ").Append(blockCurrentWaypointUntil - currentTime).Append("s)");
*
* for (var i = 1; i < _reservations[botNormal].Count - 1; i++)
* tmp.Append(_waypointIds[_reservations[botNormal][i].Node].ID).Append(";");
* botNormal.Instance.Configuration.LogAction(tmp.ToString());
* if (tmp.ToString().Equals(_lastWaitLog))
* botNormal.Instance.Configuration.LogAction("Same Log(for Debugging)");
* _lastWaitLog = tmp.ToString();
*/
//(re)add intervals
_reservationTable.Add(_reservations[botNormal]);
return(free);
}
/// <summary>
/// Find the path for all the agents.
/// </summary>
/// <param name="agents">agents</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>
public override void FindPaths(double currentTime, List <Agent> agents)
{
Stopwatch.Restart();
//Reservation Table
_reservationTable.Reorganize(currentTime);
//remove all agents that are not affected by this search
foreach (var missingAgentId in _calculatedReservations.Keys.Where(id => !agents.Any(a => a.ID == id)))
{
_reservationTable.Remove(_calculatedReservations[missingAgentId]);
}
//sort Agents
agents = agents.OrderBy(a => a.CanGoThroughObstacles ? 1 : 0).ThenBy(a => Graph.getDistance(a.NextNode, a.DestinationNode)).ToList();
Dictionary <int, double> bias = new Dictionary <int, double>();
//deadlock handling
if (UseDeadlockHandler)
{
_deadlockHandler.LengthOfAWaitStep = LengthOfAWaitStep;
_deadlockHandler.MaximumWaitTime = 30;
_deadlockHandler.Update(agents, currentTime);
}
//optimize Path
if (UseBias)
{
foreach (var agent in agents.Where(a => a.RequestReoptimization && !a.FixedPosition && a.NextNode != a.DestinationNode))
{
//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 == null || 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);
}
if (rraStars[agent.ID].Closed.Contains(agent.NextNode) || rraStars[agent.ID].Search(agent.NextNode))
{
var nodes = rraStars[agent.ID].getPathAsNodeList(agent.NextNode);
nodes.Add(agent.NextNode);
foreach (var node in nodes)
{
if (!bias.ContainsKey(node))
{
bias.Add(node, 0.0);
}
bias[node] += LengthOfAWaitStep * 1.0001;
}
}
}
}
//optimize Path
foreach (var agent in agents.Where(a => a.RequestReoptimization && !a.FixedPosition && a.NextNode != a.DestinationNode))
{
//runtime exceeded
if (Stopwatch.ElapsedMilliseconds / 1000.0 > RuntimeLimitPerAgent * agents.Count * 0.9 || Stopwatch.ElapsedMilliseconds / 1000.0 > RunTimeLimitOverall)
{
Communicator.SignalTimeout();
return;
}
//remove existing reservations of previous calculations that are not a victim of the reorganization
_reservationTable.Remove(_calculatedReservations[agent.ID]);
//all reservations deleted
_calculatedReservations[agent.ID].Clear();
if (!UseBias)
{
//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 == null || 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]);
aStar.FinalReservation = true;
List <int> nodes = null;
if (UseBias)
{
aStar.BiasedCost = bias;
if (rraStars[agent.ID].Closed.Contains(agent.NextNode) || rraStars[agent.ID].Search(agent.NextNode))
{
//subtract my own cost
nodes = rraStars[agent.ID].getPathAsNodeList(agent.NextNode);
nodes.Add(agent.NextNode);
foreach (var node in nodes)
{
bias[node] -= LengthOfAWaitStep * 1.0001;
}
}
}
//execute
var found = aStar.Search();
if (UseBias && nodes != null)
{
//re add my own cost
foreach (var node in nodes)
{
bias[node] += LengthOfAWaitStep * 1.0001;
}
}
if (!found)
{
//set a fresh reservation for my current node
_reservationTable.Clear(agent.NextNode);
_calculatedReservations[agent.ID] = new List <ReservationTable.Interval>(new ReservationTable.Interval[] { new ReservationTable.Interval(agent.NextNode, 0, double.PositiveInfinity) });
_reservationTable.Add(_calculatedReservations[agent.ID]);
agent.Path = new Path();
//clear all reservations of other agents => they will not calculate a path over this node anymore
foreach (var otherAgent in _calculatedReservations.Keys.Where(id => id != agent.ID))
{
_calculatedReservations[otherAgent].RemoveAll(r => r.Node == agent.NextNode);
}
//add wait step
agent.Path.AddFirst(agent.NextNode, true, LengthOfAWaitStep);
//add the next node again
if (agent.ReservationsToNextNode.Count > 0 && (agent.Path.Count == 0 || agent.Path.NextAction.Node != agent.NextNode || agent.Path.NextAction.StopAtNode == false))
{
agent.Path.AddFirst(agent.NextNode, true, 0);
}
continue;
}
//just wait where you are and until the search time reaches currentTime + LenghtOfAWindow is always a solution
Debug.Assert(found);
//+ WHCA* Nodes
agent.Path = new Path();
List <ReservationTable.Interval> reservations;
aStar.GetPathAndReservations(ref agent.Path, out reservations);
_calculatedReservations[agent.ID] = reservations;
//add to reservation table
_reservationTable.Add(_calculatedReservations[agent.ID]);
//add reservation to infinity
var lastNode = (_calculatedReservations[agent.ID].Count > 0) ? _calculatedReservations[agent.ID][_calculatedReservations[agent.ID].Count - 1].Node : agent.NextNode;
var lastTime = (_calculatedReservations[agent.ID].Count > 0) ? _calculatedReservations[agent.ID][_calculatedReservations[agent.ID].Count - 1].End : currentTime;
_calculatedReservations[agent.ID].Add(new ReservationTable.Interval(lastNode, lastTime, double.PositiveInfinity));
try
{
_reservationTable.Add(lastNode, lastTime, double.PositiveInfinity);
}
catch (DisjointIntervalTree.IntervalIntersectionException)
{
//This could technically fail => especially when they come from a station
}
//add the next node again
if (agent.ReservationsToNextNode.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);
}
}
}
}
/// <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);
}
}
/// <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">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="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="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);
}