/// <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 PASMethod(Graph graph, int seed, PathPlanningCommunicator logger)
: base(graph, seed, logger)
{
if (graph.BackwardEdges == null)
{
graph.GenerateBackwardEgdes();
}
rraStars = new Dictionary <int, ReverseResumableAStar>();
_reservationTable = new ReservationTable(graph, true, true, true);
}
/// <summary>
/// Initializes a new instance of the <see cref="FARMethod"/> class.
/// </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 DummyMethod(Graph graph, int seed, PathPlanningCommunicator logger)
: base(graph, seed, logger)
{
if (graph.BackwardEdges == null)
{
graph.GenerateBackwardEgdes();
}
_deadlockHandler = new DeadlockHandler(graph, seed);
_blocked = new HashSet <int>();
}
/// <summary>
/// Initializes a new instance of the <see cref="FARMethod"/> class.
/// </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 CBSMethod(Graph graph, int seed, PathPlanningCommunicator logger)
: base(graph, seed, logger)
{
if (graph.BackwardEdges == null)
{
graph.GenerateBackwardEgdes();
}
_reservationTable = new ReservationTable(graph);
_agentReservationTable = new ReservationTable(graph, false, true, false);
_deadlockHandler = new DeadlockHandler(graph, seed);
}
/// <summary>
/// Initializes a new instance of the <see cref="FARMethod"/> class.
/// </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 ODIDMethod(Graph graph, int seed, PathPlanningCommunicator logger)
: base(graph, seed, logger)
{
if (graph.BackwardEdges == null)
{
graph.GenerateBackwardEgdes();
}
//create Algorithms
_independenceDetection = new IndependenceDetection(graph, seed, null);
}
/// <summary>
/// Initializes a new instance of the <see cref="FARMethod"/> class.
/// </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 BCPMethod(Graph graph, int seed, PathPlanningCommunicator logger)
: base(graph, seed, logger)
{
if (graph.BackwardEdges == null)
{
graph.GenerateBackwardEgdes();
}
_reservationTable = new ReservationTable(graph, true, true, false);
_biasedCost = new Dictionary <int, Dictionary <int, double> >();
_deadlockHandler = new DeadlockHandler(graph, seed);
}
/// <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 WHCAvStarMethod(Graph graph, int seed, PathPlanningCommunicator logger)
: base(graph, seed, logger)
{
if (graph.BackwardEdges == null)
{
graph.GenerateBackwardEgdes();
}
rraStars = new Dictionary <int, ReverseResumableAStar>();
_reservationTable = new ReservationTable(graph);
if (UseDeadlockHandler)
{
_deadlockHandler = new DeadlockHandler(graph, seed);
}
}
/// <summary>
/// Initializes a new instance of the <see cref="FARMethod"/> class.
/// </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 FARMethod(Graph graph, int seed, EvadingStrategy evadingStragety, PathPlanningCommunicator logger)
: base(graph, seed, logger)
{
_evadingStragety = evadingStragety;
if (graph.BackwardEdges == null)
{
graph.GenerateBackwardEgdes();
}
_waitFor = new Dictionary <int, int>();
_reservationTable = new ReservationTable(graph, false, true, false);
_rraStar = new Dictionary <int, ReverseResumableAStar>();
_moveTime = new Dictionary <int, double>();
_waitTime = new Dictionary <int, double>();
_es2evadedFrom = new Dictionary <int, HashSet <int> >();
if (UseDeadlockHandler)
{
_deadlockHandler = new DeadlockHandler(graph, seed);
}
}
/// <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>
/// 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]);
}
}
}
}
