/// <summary>
/// When equivalence over different times is necessary,
/// checks this.agent and last position only,
/// ignoring data that would make this state different to other equivalent states:
/// It doesn't matter from which direction the agent got to its current location.
/// It's also necessary to ignore the agents' move time - we want the same positions
/// in any time to be equivalent.
/// </summary>
/// <param name="obj"></param>
/// <returns></returns>
public override bool Equals(object obj)
{
AgentState that = (AgentState)obj;
if (AgentState.EquivalenceOverDifferentTimes)
{
return(this.agent.Equals(that.agent) &&
this.lastMove.x == that.lastMove.x &&
this.lastMove.y == that.lastMove.y); // Ignoring the time and the direction
}
else
{
return(this.agent.Equals(that.agent) &&
this.lastMove.x == that.lastMove.x &&
this.lastMove.y == that.lastMove.y &&
this.lastMove.time == that.lastMove.time); // Ignoring the direction
}
}
/// <summary>
/// Used when AgentState objects are put in the open list priority queue - mainly in AStarForSingleAgent, I think.
/// </summary>
/// <param name="other"></param>
/// <returns></returns>
public int CompareTo(IBinaryHeapItem other)
{
AgentState that = (AgentState)other;
if (this.h + this.lastMove.time < that.h + that.lastMove.time)
{
return(-1);
}
if (this.h + this.lastMove.time > that.h + that.lastMove.time)
{
return(1);
}
if (this.potentialConflictsID < that.potentialConflictsID)
{
return(-1);
}
if (this.potentialConflictsID > that.potentialConflictsID)
{
return(1);
}
if (this.potentialConflicts < that.potentialConflicts) // Doesn't this come before the potentialConflictsID in other places?
{
return(-1);
}
if (this.potentialConflicts > that.potentialConflicts)
{
return(1);
}
// TODO: Prefer goal nodes.
// Prefer larger g:
if (this.lastMove.time < that.lastMove.time)
{
return(1);
}
if (this.lastMove.time > that.lastMove.time)
{
return(-1);
}
return(0);
}
/// <summary>
/// The returned plan wasn't constructed considering a CAT, so it's possible there's an alternative plan with the same cost and less collisions.
/// </summary>
/// <param name="agentState"></param>
/// <returns></returns>
public SinglePlan GetSingleAgentOptimalPlan(AgentState agentState,
out Dictionary <int, int> conflictCountPerAgent, out Dictionary <int, List <int> > conflictTimesPerAgent)
{
LinkedList <Move> moves = new LinkedList <Move>();
int agentNum = agentState.agent.agentNum;
var conflictCounts = new Dictionary <int, int>();
var conflictTimes = new Dictionary <int, List <int> >();
IReadOnlyDictionary <TimedMove, List <int> > CAT;
if (this.parameters.ContainsKey(CBS_LocalConflicts.CAT)) // TODO: Add support for IndependenceDetection's CAT
{
CAT = ((IReadOnlyDictionary <TimedMove, List <int> >) this.parameters[CBS_LocalConflicts.CAT]);
}
else
{
CAT = new Dictionary <TimedMove, List <int> >();
}
TimedMove current = agentState.lastMove; // The starting position
int time = current.time;
while (true)
{
moves.AddLast(current);
// Count conflicts:
current.UpdateConflictCounts(CAT, conflictCounts, conflictTimes);
if (agentState.agent.Goal.Equals(current))
{
break;
}
// Get next optimal move
time++;
Move optimal = this.singleAgentOptimalMoves[agentNum][this.GetCardinality(current)];
current = new TimedMove(optimal, time);
}
conflictCountPerAgent = conflictCounts;
conflictTimesPerAgent = conflictTimes;
return(new SinglePlan(moves, agentNum));
}
private bool singleAgentA_Star(AgentState agent)
{
BinaryHeap openList = new BinaryHeap(); // TODO: Safe to use OpenList here instead?
HashSet <AgentState> closedList = new HashSet <AgentState>();
openList.Add(agent);
AgentState temp = agent;
bool valid;
while (openList.Count > 0)
{
if (this.runner.ElapsedMilliseconds() > 120000)
{
return(false);
}
temp = (AgentState)openList.Remove();
if (temp.h == 0)
{
valid = true;
for (int i = temp.lastMove.time; i <= maxPathCost; i++)
{
if (RT.Contains(new TimedMove(temp.lastMove.x, temp.lastMove.y, Move.Direction.NO_DIRECTION, i)))
{
valid = false;
}
}
if (valid)
{
reservePath(temp);
totalTime += temp.lastMove.time;
//printPath(temp);
parked.Add(new Move(temp.lastMove.x, temp.lastMove.y, Move.Direction.NO_DIRECTION), temp.lastMove.time);
return(true);
}
}
expanded++;
expendNode(temp, openList, closedList);
}
return(false);
}
private void expendNode(AgentState node, BinaryHeap openList, HashSet <AgentState> closedList)
{
foreach (TimedMove move in node.lastMove.GetNextMoves())
{
if (isValidMove(move))
{
AgentState temp = new AgentState(node);
AgentState tempCL = temp;
temp.prev = node;
if (move.time > maxPathCost)
{
tempCL = new AgentState(temp);
tempCL.lastMove.time = maxPathCost;
}
if (!closedList.Contains(tempCL))
{
closedList.Add(tempCL);
temp.lastMove.time = move.time;
openList.Add(temp);
generated++;
}
}
}
}
/// <summary>
/// Imports a problem instance from a given file
/// </summary>
/// <param name="fileName"></param>
/// <returns></returns>
public static ProblemInstance Import(string fileName)
{
TextReader input = new StreamReader(fileName);
//return new ProblemInstance(); // DELETE ME!!!
string[] lineParts;
string line;
int instanceId = 0;
string gridName = "Random Grid"; // The default
line = input.ReadLine();
if (line.StartsWith("Grid:") == false)
{
lineParts = line.Split(',');
instanceId = int.Parse(lineParts[0]);
if (lineParts.Length > 1)
{
gridName = lineParts[1];
}
line = input.ReadLine();
}
//instanceId = int.Parse(fileName.Split('-')[4]);
// First/second line is Grid:
Debug.Assert(line.StartsWith("Grid:"));
// Read grid dimensions
line = input.ReadLine();
lineParts = line.Split(',');
int maxX = int.Parse(lineParts[0]);
int maxY = int.Parse(lineParts[1]);
bool[][] grid = new bool[maxX][];
char cell;
for (int i = 0; i < maxX; i++)
{
try
{
grid[i] = new bool[maxY];
line = input.ReadLine();
for (int j = 0; j < maxY; j++)
{
try
{
cell = line.ElementAt(j);
if (cell == '@' || cell == 'O' || cell == 'T' || cell == 'W' /* Water isn't traversable from land */)
{
grid[i][j] = true;
}
else
{
grid[i][j] = false;
}
}
catch { }
}
}
catch { }
}
// Next line is Agents:
line = input.ReadLine();
Debug.Assert(line.StartsWith("Agents:"));
// Read the number of agents
line = input.ReadLine();
int numOfAgents = int.Parse(line);
// Read the agents' start and goal states
AgentState[] states = new AgentState[numOfAgents];
AgentState state;
AgentState newSingleState;
Agent newSingleAgent;
Agent agent;
int agentNum;
int agentNumSingle = 0;
int goalX;
int goalY;
int startX;
int startY;
// Run on each of the agents
for (int i = 0; i < numOfAgents; i++)
{
line = input.ReadLine();
lineParts = line.Split(EXPORT_DELIMITER);
agentNum = int.Parse(lineParts[0]);
goalX = int.Parse(lineParts[1]);
goalY = int.Parse(lineParts[2]);
startX = int.Parse(lineParts[3]);
startY = int.Parse(lineParts[4]);
agent = new Agent(goalX, goalY, agentNum);
state = new AgentState(startX, startY, agent);
states[i] = state;
// ******** Some of the project modifications: ********
// numberOfRun 1 is the first solver of the MAPF problem
// here we are building a list of single agent problem, it will be used to automate the calculation of the middle positions as described in the project report)
if (Program.numberOfRun == 1)
{
newSingleAgent = new Agent(goalX, goalY, agentNumSingle);
newSingleState = new AgentState(startX, startY, newSingleAgent);
AgentState[] singleStates = new AgentState[1];
singleStates[0] = newSingleState;
// Generate the single agents problem instance
ProblemInstance instanceSingle = new ProblemInstance();
instanceSingle.Init(singleStates, grid);
instanceSingle.instanceId = i;
instanceSingle.parameters[ProblemInstance.GRID_NAME_KEY] = gridName;
// Saving the single agent problems in an array
Program.onlySingleAgentsInstances[i] = instanceSingle;
}
}
// Generate the regular problem instance
ProblemInstance instance = new ProblemInstance();
instance.Init(states, grid);
instance.instanceId = instanceId;
instance.parameters[ProblemInstance.GRID_NAME_KEY] = gridName;
if (Program.numberOfRun == 1)
{
instance.ComputeSingleAgentShortestPaths();
}
return(instance);
}
/// <summary>
/// Returns the optimal move towards the goal of the given agent. Move isn't necessarily unique.
/// </summary>
/// <param name="agent"></param>
/// <returns></returns>
public Move GetSingleAgentOptimalMove(AgentState agentState)
{
return(this.singleAgentOptimalMoves[agentState.agent.agentNum][this.m_vCardinality[agentState.lastMove.x, agentState.lastMove.y]]);
}
/// <summary>
/// Compute the shortest path to the goal of every agent in the problem instance, from every location in the grid.
/// Current implementation is a simple breadth-first search from every location in the graph.
/// </summary>
public void ComputeSingleAgentShortestPaths()
{
//Debug.WriteLine("Computing the single agent shortest path for all agents...");
//return; // Add for generator
this.singleAgentOptimalCosts = new int[this.GetNumOfAgents()][];
this.singleAgentOptimalMoves = new Move[this.GetNumOfAgents()][];
for (int agentId = 0; agentId < this.GetNumOfAgents(); agentId++)
{
// Run a single source shortest path algorithm from the _goal_ of the agent
var shortestPathLengths = new int[this.m_nLocations];
var optimalMoves = new Move[this.m_nLocations];
for (int i = 0; i < m_nLocations; i++)
{
shortestPathLengths[i] = -1;
}
var openlist = new Queue <AgentState>();
// Create initial state
var agentStartState = this.m_vAgents[agentId];
var agent = agentStartState.agent;
var goalState = new AgentState(agent.Goal.x, agent.Goal.y, -1, -1, agentId);
int goalIndex = this.GetCardinality(goalState.lastMove);
shortestPathLengths[goalIndex] = 0;
optimalMoves[goalIndex] = new Move(goalState.lastMove);
openlist.Enqueue(goalState);
while (openlist.Count > 0)
{
AgentState state = openlist.Dequeue();
// Generate child states
foreach (TimedMove aMove in state.lastMove.GetNextMoves())
{
if (IsValid(aMove))
{
int entry = m_vCardinality[aMove.x, aMove.y];
// If move will generate a new or better state - add it to the queue
if ((shortestPathLengths[entry] == -1) || (shortestPathLengths[entry] > state.g + 1))
{
var childState = new AgentState(state);
childState.MoveTo(aMove);
shortestPathLengths[entry] = childState.g;
optimalMoves[entry] = new Move(aMove.GetOppositeMove());
openlist.Enqueue(childState);
}
}
}
}
int start = this.GetCardinality(agentStartState.lastMove);
if (shortestPathLengths[start] == -1)
{
throw new Exception(String.Format("Unsolvable instance! Agent {0} cannot reach its goal", agentId));
}
this.singleAgentOptimalCosts[agentId] = shortestPathLengths;
this.singleAgentOptimalMoves[agentId] = optimalMoves;
}
}
/// <summary>
/// Generates a problem instance based on a DAO map file.
/// TODO: Fix code dup with GenerateProblemInstance and Import later.
/// </summary>
/// <param name="agentsNum"></param>
/// <returns></returns>
public ProblemInstance GenerateDragonAgeProblemInstance(string mapFileName, int agentsNum)
{
/**
* Randomization based on timer is disabled for purposes of getting
* reproducible experiments.
*/
//Random rand = new Random();
m_mapFileName = mapFileName;
m_agentNum = agentsNum;
TextReader input = new StreamReader(mapFileName);
string[] lineParts;
string line;
line = input.ReadLine();
Debug.Assert(line.StartsWith("type octile"));
// Read grid dimensions
line = input.ReadLine();
lineParts = line.Split(' ');
Debug.Assert(lineParts[0].StartsWith("height"));
int maxX = int.Parse(lineParts[1]);
line = input.ReadLine();
lineParts = line.Split(' ');
Debug.Assert(lineParts[0].StartsWith("width"));
int maxY = int.Parse(lineParts[1]);
line = input.ReadLine();
Debug.Assert(line.StartsWith("map"));
bool[][] grid = new bool[maxX][];
char cell;
for (int i = 0; i < maxX; i++)
{
grid[i] = new bool[maxY];
line = input.ReadLine();
for (int j = 0; j < maxY; j++)
{
cell = line.ElementAt(j);
if (cell == '@' || cell == 'O' || cell == 'T' || cell == 'W' /* Water isn't traversable from land */)
{
grid[i][j] = true;
}
else
{
grid[i][j] = false;
}
}
}
int x;
int y;
Agent[] agentGoals = new Agent[agentsNum];
AgentState[] agentStates = new AgentState[agentsNum];
bool[][] goals = new bool[maxX][];
for (int i = 0; i < maxX; i++)
{
goals[i] = new bool[maxY];
}
// Choose random valid unclaimed goal locations
for (int i = 0; i < agentsNum; i++)
{
x = rand.Next(maxX);
y = rand.Next(maxY);
if (goals[x][y] || grid[x][y])
{
i--;
}
else
{
goals[x][y] = true;
agentGoals[i] = new Agent(x, y, i);
}
}
// Select random start/goal locations for every agent by performing a random walk
for (int i = 0; i < agentsNum; i++)
{
agentStates[i] = new AgentState(agentGoals[i].Goal.x, agentGoals[i].Goal.y, agentGoals[i]);
}
ProblemInstance problem = new ProblemInstance();
problem.parameters[ProblemInstance.GRID_NAME_KEY] = Path.GetFileNameWithoutExtension(mapFileName);
problem.Init(agentStates, grid);
for (int j = 0; j < RANDOM_WALK_STEPS; j++)
{
for (int i = 0; i < agentsNum; i++)
{
goals[agentStates[i].lastMove.x][agentStates[i].lastMove.y] = false; // We're going to move the goal somewhere else
// Move in a random legal direction:
while (true)
{
Move.Direction op = (Move.Direction)rand.Next(0, 5); // TODO: fixme
agentStates[i].lastMove.Update(op);
if (problem.IsValid(agentStates[i].lastMove) &&
!goals[agentStates[i].lastMove.x][agentStates[i].lastMove.y]) // This spot isn't another agent's goal
{
break;
}
else
{
agentStates[i].lastMove.setOppositeMove(); // Rollback
}
}
goals[agentStates[i].lastMove.x][agentStates[i].lastMove.y] = true; // Claim agent's new goal
}
}
// Zero the agents' timesteps
foreach (AgentState agentStart in agentStates)
{
agentStart.lastMove.time = 0;
}
return(problem);
}
/// <summary>
/// Generates a problem instance, including a board, start and goal locations of desired number of agents
/// and desired precentage of obstacles
/// TODO: Refactor to use operators.
/// </summary>
/// <param name="gridSize"></param>
/// <param name="agentsNum"></param>
/// <param name="obstaclesNum"></param>
/// <returns></returns>
public ProblemInstance GenerateProblemInstance(int gridSize, int agentsNum, int obstaclesNum)
{
m_mapFileName = "GRID" + gridSize + "X" + gridSize;
m_agentNum = agentsNum;
/**
* Randomization based on timer is disabled for purposes of getting
* reproducible experiments.
*/
//Random rand = new Random();
if (agentsNum + obstaclesNum + 1 > gridSize * gridSize)
{
throw new Exception("Not enough room for " + agentsNum + ", " + obstaclesNum + " and one empty space in a " + gridSize + "x" + gridSize + "map.");
}
int x;
int y;
Agent[] aGoals = new Agent[agentsNum];
AgentState[] aStart = new AgentState[agentsNum];
bool[][] grid = new bool[gridSize][];
bool[][] goals = new bool[gridSize][];
// Generate a random grid
for (int i = 0; i < gridSize; i++)
{
grid[i] = new bool[gridSize];
goals[i] = new bool[gridSize];
}
for (int i = 0; i < obstaclesNum; i++)
{
x = rand.Next(gridSize);
y = rand.Next(gridSize);
if (grid[x][y]) // Already an obstacle
{
i--;
}
grid[x][y] = true;
}
// Choose random goal locations
for (int i = 0; i < agentsNum; i++)
{
x = rand.Next(gridSize);
y = rand.Next(gridSize);
if (goals[x][y] || grid[x][y])
{
i--;
}
else
{
goals[x][y] = true;
aGoals[i] = new Agent(x, y, i);
}
}
// Select random start/goal locations for every agent by performing a random walk
for (int i = 0; i < agentsNum; i++)
{
aStart[i] = new AgentState(aGoals[i].Goal.x, aGoals[i].Goal.y, aGoals[i]);
}
// Initialized here only for the IsValid() call. TODO: Think how this can be sidestepped elegantly.
ProblemInstance problem = new ProblemInstance();
problem.Init(aStart, grid);
for (int j = 0; j < RANDOM_WALK_STEPS; j++)
{
for (int i = 0; i < agentsNum; i++)
{
goals[aStart[i].lastMove.x][aStart[i].lastMove.y] = false; // We're going to move the goal somewhere else
while (true)
{
Move.Direction op = (Move.Direction)rand.Next(0, 5); // TODO: fixme
aStart[i].lastMove.Update(op);
if (problem.IsValid(aStart[i].lastMove) &&
!goals[aStart[i].lastMove.x][aStart[i].lastMove.y]) // this spot isn't another agent's goal
{
break;
}
else
{
aStart[i].lastMove.setOppositeMove(); // Rollback
}
}
goals[aStart[i].lastMove.x][aStart[i].lastMove.y] = true; // Claim agent's new goal
}
}
// Zero the agents' timesteps
foreach (AgentState agentStart in aStart)
{
agentStart.lastMove.time = 0;
}
// TODO: There is some repetition here of previous instantiation of ProblemInstance. Think how to elegantly bypass this.
problem = new ProblemInstance();
problem.Init(aStart, grid);
return(problem);
}
/// <summary>
/// Imports a problem instance from a given file
/// </summary>
/// <param name="fileName"></param>
/// <returns></returns>
public static ProblemInstance Import(string fileName)
{
TextReader input = new StreamReader(fileName);
string[] lineParts;
string line;
int instanceId = 0;
string gridName = "Random Grid"; // The default
line = input.ReadLine();
if (line.StartsWith("Grid:") == false)
{
lineParts = line.Split(',');
instanceId = int.Parse(lineParts[0]);
if (lineParts.Length > 1)
{
gridName = lineParts[1];
}
line = input.ReadLine();
}
//instanceId = int.Parse(fileName.Split('-')[4]);
// First/second line is Grid:
Debug.Assert(line.StartsWith("Grid:"));
// Read grid dimensions
line = input.ReadLine();
lineParts = line.Split(',');
int maxX = int.Parse(lineParts[0]);
int maxY = int.Parse(lineParts[1]);
bool[][] grid = new bool[maxX][];
char cell;
for (int i = 0; i < maxX; i++)
{
grid[i] = new bool[maxY];
line = input.ReadLine();
for (int j = 0; j < maxY; j++)
{
cell = line.ElementAt(j);
if (cell == '@' || cell == 'O' || cell == 'T' || cell == 'W' /* Water isn't traversable from land */)
{
grid[i][j] = true;
}
else
{
grid[i][j] = false;
}
}
}
// Next line is Agents:
line = input.ReadLine();
Debug.Assert(line.StartsWith("Agents:"));
// Read the number of agents
line = input.ReadLine();
int numOfAgents = int.Parse(line);
// Read the agents' start and goal states
AgentState[] states = new AgentState[numOfAgents];
AgentState state;
Agent agent;
int agentNum;
int goalX;
int goalY;
int startX;
int startY;
for (int i = 0; i < numOfAgents; i++)
{
line = input.ReadLine();
lineParts = line.Split(EXPORT_DELIMITER);
agentNum = int.Parse(lineParts[0]);
goalX = int.Parse(lineParts[1]);
goalY = int.Parse(lineParts[2]);
startX = int.Parse(lineParts[3]);
startY = int.Parse(lineParts[4]);
agent = new Agent(goalX, goalY, agentNum);
state = new AgentState(startX, startY, agent);
states[i] = state;
}
// Generate the problem instance
ProblemInstance instance = new ProblemInstance();
instance.Init(states, grid);
instance.instanceId = instanceId;
instance.parameters[ProblemInstance.GRID_NAME_KEY] = gridName;
instance.ComputeSingleAgentShortestPaths();
return(instance);
}
/// <summary>
/// Returns the length of the shortest path between a given agent's location and the goal of that agent.
/// </summary>
/// <param name="agent"></param>
/// <returns>The length of the shortest path between a given agent's location and the goal of that agent</returns>
public int GetSingleAgentOptimalCost(AgentState agent)
{
return(this.singleAgentOptimalCosts[agent.agent.agentNum][this.m_vCardinality[agent.lastMove.x, agent.lastMove.y]]);
}
