private bool singleAgentAStar(AgentState agent)
{
AgentState.EquivalenceOverDifferentTimes = false;
BinaryHeap <AgentState> openList = new BinaryHeap <AgentState>(); // TODO: Safe to use OpenList here instead?
HashSet <AgentState> closedList = new HashSet <AgentState>();
agent.h = this.problem.GetSingleAgentOptimalCost(agent);
openList.Add(agent);
AgentState node;
this.initialEstimate += agent.h;
TimedMove queryTimedMove = new TimedMove();
while (openList.Count > 0)
{
if (this.runner.ElapsedMilliseconds() > Constants.MAX_TIME)
{
return(false);
}
node = openList.Remove();
if (node.h == 0)
{
bool valid = true;
for (int i = node.lastMove.time; i <= maxPathCostSoFar; i++)
{
queryTimedMove.setup(node.lastMove.x, node.lastMove.y, Move.Direction.NO_DIRECTION, i);
if (reservationTable.Contains(queryTimedMove))
{
valid = false;
}
}
if (valid)
{
this.paths[agent.agent.agentNum] = new SinglePlan(node);
reservePath(node);
totalcost += node.lastMove.time;
parked.Add(new Move(node.lastMove.x, node.lastMove.y, Move.Direction.NO_DIRECTION), node.lastMove.time);
return(true);
}
}
expandNode(node, openList, closedList);
expanded++;
}
return(false);
}
/// <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);
}
private void expandNode(AgentState node, BinaryHeap <AgentState> openList, HashSet <AgentState> closedList)
{
foreach (TimedMove move in node.lastMove.GetNextMoves())
{
if (this.isValidMove(move))
{
AgentState child = new AgentState(node);
child.prev = node;
child.MoveTo(move);
if (closedList.Contains(child) == false)
{
closedList.Add(child);
child.h = this.problem.GetSingleAgentOptimalCost(child);
openList.Add(child);
generated++;
}
}
}
}
/// <summary>
/// Computes the shortest path to the goal for a given agent from every location in the grid.
/// Current implementation is a simple breadth-first search from every location in the graph.
/// </summary>
/// <param name="state">Agent's goal state</param>
/// <returns>Tuple with shortestPathLengths and optimalMoves </returns>
public Tuple <int[], Move[]> AllShortestPathsTo(AgentState state)
{
var openlist = new Queue <AgentState>();
var shortestPathLengths = new int[this.numLocations];
var optimalMoves = new Move[this.numLocations];
for (int i = 0; i < numLocations; i++)
{
shortestPathLengths[i] = -1;
}
openlist.Enqueue(state);
int goalIndex = this.GetCardinality(state.lastMove);
shortestPathLengths[goalIndex] = 0;
optimalMoves[goalIndex] = new Move(state.lastMove);
while (openlist.Count > 0)
{
AgentState nextState = openlist.Dequeue();
// Generate child states
foreach (TimedMove aMove in nextState.lastMove.GetNextMoves())
{
if (IsValid(aMove))
{
int entry = cardinality[aMove.x, aMove.y];
// If move will generate a new or better state - add it to the queue
if ((shortestPathLengths[entry] == -1) || (shortestPathLengths[entry] > nextState.g + 1))
{
var childState = new AgentState(nextState);
childState.MoveTo(aMove);
shortestPathLengths[entry] = childState.g;
optimalMoves[entry] = new Move(aMove.GetOppositeMove());
openlist.Enqueue(childState);
}
}
}
}
return(Tuple.Create <int[], Move[]>(shortestPathLengths, optimalMoves));
}
/// <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)
{
if (obj == null)
{
return(false);
}
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>
/// Note: 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>An optimal plan for the agent, ignoring all others</returns>
public SinglePlan GetSingleAgentOptimalPlan(AgentState agentState)
{
LinkedList <Move> moves = new LinkedList <Move>();
int agentNum = agentState.agent.agentNum;
TimedMove current = agentState.lastMove; // The starting position
int time = current.time;
while (true)
{
moves.AddLast(current);
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);
}
return(new SinglePlan(moves, agentNum));
}
/// <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()
{
Console.WriteLine("Computing the single agent shortest path for all agents...");
Stopwatch watch = Stopwatch.StartNew();
double startTime = watch.Elapsed.TotalMilliseconds;
//return; // Add for generator
for (int agentId = 0; agentId < this.GetNumOfAgents(); agentId++)
{
int[] shortestPathLengths;
Move[] optimalMoves;
AgentState agentStartState = this.agents[agentId];
int start;
if (this.agentDistancesToGoal[agentId] == 0)
{
singleAgentShortestPath(agentId, out shortestPathLengths, out optimalMoves, out agentStartState, out start);
this.agentDistancesToGoal[agentId] = shortestPathLengths[start];
this.singleAgentOptimalCosts[agentId] = shortestPathLengths;
this.singleAgentOptimalMoves[agentId] = optimalMoves;
}
//for (int otherAgentId = 0; otherAgentId < this.GetNumOfAgents(); otherAgentId++)
//{
// var otherAgentState = this.agents[otherAgentId];
// if (this.distanceBetweenAgentGoals[agentId, otherAgentId] != 0)
// continue; // skip already computed
// this.distanceBetweenAgentGoals[agentId, otherAgentId] = GetSingleAgentOptimalCost(agentId, otherAgentState.agent.Goal); //Distance from this agent to other agent goal
// this.distanceBetweenAgentStartPoints[agentId, otherAgentId] = ShortestPathFromAToB(agentStartState, otherAgentState.lastMove);
//} TODO: Delete that, made a more efficent C-based implemetation for feature extraction
}
double endTime = watch.Elapsed.TotalMilliseconds;
this.shortestPathComputeTime = endTime - startTime;
Console.WriteLine("Time to calc sp: {0}", shortestPathComputeTime);
}
/// <summary>
/// Imports a problem instance from a given file
/// </summary>
/// <param name="filePath"></param>
/// <param name="mapFilePath"></param>
/// <returns></returns>
public static ProblemInstance Import(string filePath, string mapFilePath = null)
{
if (filePath.EndsWith(".agents"))
{
string fileNameWithoutExtension = Path.GetFileNameWithoutExtension(filePath);
int instanceId = 0;
string mapfileNameWithoutExtension;
if (mapFilePath == null)
{
mapfileNameWithoutExtension = fileNameWithoutExtension.Substring(0, length: fileNameWithoutExtension.LastIndexOf('_')) + ".map"; // Passing a length parameter is like specifying a non-inclusive end index
mapFilePath = Path.Combine(Path.GetDirectoryName(filePath), "..", "maps", mapfileNameWithoutExtension);
instanceId = int.Parse(filePath.Split('_').Last());
}
else
{
mapfileNameWithoutExtension = Path.GetFileNameWithoutExtension(mapFilePath);
}
bool[][] grid;
string line;
string[] lineParts;
using (TextReader input = new StreamReader(mapFilePath))
{
// Read grid dimensions
line = input.ReadLine();
lineParts = line.Split(',');
int maxX = int.Parse(lineParts[0]);
int maxY = int.Parse(lineParts[1]);
grid = new bool[maxX][];
char cell;
// Read grid
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 == '1')
{
grid[i][j] = true;
}
else
{
grid[i][j] = false;
}
}
}
}
AgentState[] states;
using (TextReader input = new StreamReader(filePath))
{
// Read the number of agents
line = input.ReadLine();
int numOfAgents = int.Parse(line);
// Read the agents' start and goal states
states = new AgentState[numOfAgents];
AgentState state;
Agent agent;
int goalX;
int goalY;
int startX;
int startY;
for (int i = 0; i < numOfAgents; i++)
{
line = input.ReadLine();
lineParts = line.Split(EXPORT_DELIMITER);
goalX = int.Parse(lineParts[0]);
goalY = int.Parse(lineParts[1]);
startX = int.Parse(lineParts[2]);
startY = int.Parse(lineParts[3]);
agent = new Agent(goalX, goalY, i);
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] = mapfileNameWithoutExtension;
instance.parameters[ProblemInstance.INSTANCE_NAME_KEY] = fileNameWithoutExtension + ".agents";
instance.ComputeSingleAgentShortestPaths();
return(instance);
}
else if (filePath.EndsWith(".scen"))
{
return(ImportFromScenFile(filePath));
}
else // Combined map and scenario, no suffix
{
using (TextReader input = new StreamReader(filePath))
{
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();
}
// 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][];
// Read grid
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.parameters[ProblemInstance.INSTANCE_NAME_KEY] = Path.GetFileName(filePath);
instance.ComputeSingleAgentShortestPaths();
return(instance);
}
}
}
public static ProblemInstance ImportFromScenFile(string fileName)
{
string fileNameWithoutExtension = Path.GetFileNameWithoutExtension(fileName);
int instanceId = int.Parse(fileNameWithoutExtension.Split('-').Last());
//string scen_type = "";
string mapfileName = "";
int scen_type_seperator_index = 0;
if (fileNameWithoutExtension.Split('-')[0].Contains("empty")) //empty map
{
scen_type_seperator_index = 2;
}
else if (fileNameWithoutExtension.Split('-')[0].Contains("maze") || fileNameWithoutExtension.Split('-')[0].Contains("random") ||
fileNameWithoutExtension.Split('-')[0].Contains("room"))
{
scen_type_seperator_index = 3;
}
else if (fileNameWithoutExtension.Split('-')[0].Contains("warehouse"))
{
scen_type_seperator_index = 5;
}
mapfileName = fileNameWithoutExtension.Substring(0, length: IndexOfNth(fileNameWithoutExtension, "-", scen_type_seperator_index));
Console.WriteLine(mapfileName);
string mapFilePath = Path.Combine(Path.GetDirectoryName(fileName), @"..", @"..", "maps", mapfileName + ".map");
Console.WriteLine("map file path {0} {1}", Path.GetDirectoryName(fileName), mapFilePath);
bool[][] grid;
string line;
string[] lineParts;
int maxX;
int maxY;
using (TextReader input = new StreamReader(mapFilePath))
{
// Read grid dimensions
line = input.ReadLine();
Debug.Assert(line.StartsWith("type octile"));
line = input.ReadLine();
lineParts = line.Split(' ');
Trace.Assert(lineParts.Length == 2);
//Trace.Assert(lineParts[0].Equals("height"));
maxY = int.Parse(lineParts[1]); // The height is the number of rows
line = input.ReadLine();
lineParts = line.Split(' ');
Trace.Assert(lineParts.Length == 2);
//Trace.Assert(lineParts[0].Equals("width"));
maxX = int.Parse(lineParts[1]); // The width is the number of columns
grid = new bool[maxY][];
line = input.ReadLine();
Trace.Assert(line.StartsWith("map"));
char cell;
// Read grid
for (int i = 0; i < maxY; i++)
{
grid[i] = new bool[maxX];
line = input.ReadLine();
for (int j = 0; j < maxX; j++)
{
cell = line[j];
if (cell == '@' || cell == 'O' || cell == 'T' || cell == 'W' /* Water isn't traversable from land */)
{
grid[i][j] = true;
}
else
{
grid[i][j] = false;
}
}
}
}
List <AgentState> stateList = new List <AgentState>();
Run runner = new Run();
Console.WriteLine("Starting scen file {0}", fileName);
var rnd = new Random();
var filename_without_extension = fileName.Substring(0, fileName.IndexOf(".scen"));
var agents_fileName = filename_without_extension + ".agents";
var plan_fileName = filename_without_extension + ".plans";
//if (File.Exists(agents_fileName))
// File.Delete(agents_fileName);
using (TextReader input = new StreamReader(fileName))
{
// Read the format version number
line = input.ReadLine();
lineParts = line.Split(' ');
Debug.Assert(lineParts[0].Equals("version"));
int version = int.Parse(lineParts[1]);
Debug.Assert(version == 1, "Only version 1 is currently supported");
// Read the agents' start and goal states
AgentState state;
Agent agent;
int agentNum = 0;
int block;
int goalX;
int goalY;
int startX;
int startY;
string mapFileName;
int mapRows;
int mapCols;
double optimalCost; // Assuming diagonal moves are allowed and cost sqrt(2)
List <string> lines = new List <string>();
ProblemInstance instance = new ProblemInstance();
while (true)
{
line = input.ReadLine();
if (string.IsNullOrWhiteSpace(line))
{
break;
}
lines.Add(line);
}
Console.WriteLine("Found {0} agents", lines.Count);
for (int i = 2; i < lines.Count + 1; i++)
{
agentNum = 0;
if (i > 1000)
{
break;
}
stateList = new List <AgentState>();
//var rand_lines = lines.AsEnumerable().OrderBy(n => Guid.NewGuid()).Take(i).Cast<String>().ToList();
var rand_lines = lines.Take(i);
if (File.Exists(agents_fileName))
{
bool already_executed = File.ReadLines(agents_fileName)
.Any(curr_line => curr_line.Contains("NumAgents: " + i));
if (already_executed)
{
Console.WriteLine("Skipping already solved problem with {0} agents", i);
continue;
}
}
var agents_writer = new StreamWriter(agents_fileName, true);
agents_writer.WriteLine("NumAgents: {0}", i);
foreach (String rand_line in rand_lines)
{
lineParts = rand_line.Split('\t');
block = int.Parse(lineParts[0]);
mapFileName = lineParts[1];
mapRows = int.Parse(lineParts[2]);
Debug.Assert(mapRows == maxX);
mapCols = int.Parse(lineParts[3]);
Debug.Assert(mapCols == maxY);
startY = int.Parse(lineParts[4]);
startX = int.Parse(lineParts[5]);
goalY = int.Parse(lineParts[6]);
goalX = int.Parse(lineParts[7]);
optimalCost = double.Parse(lineParts[8]);
agent = new Agent(goalX, goalY, agentNum);
state = new AgentState(startX, startY, agent);
stateList.Add(state);
agents_writer.WriteLine("{0},{1},{2},{3}", startX, startY, goalX, goalY);
agentNum++;
}
agents_writer.Close();
bool resultsFileExisted = File.Exists(Program.RESULTS_FILE_NAME);
runner.OpenResultsFile(Program.RESULTS_FILE_NAME);
if (resultsFileExisted == false)
{
runner.PrintResultsFileHeader();
}
runner.CloseResultsFile();
agents_writer = new StreamWriter(agents_fileName, true);
Console.WriteLine("Starting scen with {0} agents", i);
// Generate the problem instance
/////------------- Generate SAT file from scen+map
var scen_files_dir = Directory.GetParent(filename_without_extension);
var sat_mpf_fileName = Path.Combine(scen_files_dir.ToString(), "sat_files", Path.GetFileName(filename_without_extension)
+ String.Format("_a_{0}.mpf", i));
/////------------- Generate SAT file from scen+map
if (instance.agents == null) //Should init the ProblemInstance
{
instance.Init(stateList.ToArray(), grid);
instance.ComputeSingleAgentShortestPaths();
}
else
{
instance.AddSingleAgent(stateList.Last());
}
var lazyCbsAgentsFileName = Path.Combine(scen_files_dir.ToString(), "lazycbs", Path.GetFileName(filename_without_extension)
+ String.Format("_a_{0}.agents", i));
string lazyCbsMapFileName = Path.Combine(Path.GetDirectoryName(fileName), @"..", @"..", "maps", mapfileName + ".map.ecbs");
instance.parameters[ProblemInstance.SAT_FILE_NAME] = sat_mpf_fileName;
instance.parameters[ProblemInstance.MAP_FILE_PATH] = mapFilePath;
instance.parameters[ProblemInstance.SCEN_FILE] = fileName;
instance.parameters[ProblemInstance.LAZY_CBS_AGENTS_FILE_NAME] = lazyCbsAgentsFileName;
instance.parameters[ProblemInstance.LAZY_CBS_MAP_FILE_NAME] = lazyCbsMapFileName;
instance.parameters[ProblemInstance.SCEN_DIR] = scen_files_dir;
instance.parameters[ProblemInstance.N_AGENTS] = i;
instance.instanceId = instanceId;
instance.parameters[ProblemInstance.GRID_NAME_KEY] = mapfileName;
instance.parameters[ProblemInstance.INSTANCE_NAME_KEY] = fileNameWithoutExtension + ".scen";
runner.OpenResultsFile(Program.RESULTS_FILE_NAME);
Boolean solved = runner.SolveGivenProblem(instance, plan_fileName);
runner.CloseResultsFile();
agents_writer.Close();
if (!solved)
{
break;
}
}
}
return(null);
}
/// <summary>
/// Returns the optimal move towards the goal of the given agent. Move isn't necessarily unique.
/// </summary>
/// <param name="agentState"></param>
/// <returns></returns>
public Move GetSingleAgentOptimalMove(AgentState agentState)
{
int locationCardinality = this.cardinality[agentState.lastMove.x, agentState.lastMove.y];
return(this.singleAgentOptimalMoves[agentState.agent.agentNum][locationCardinality]);
}
private void singleAgentShortestPath(int agentId, out int[] shortestPathLengths, out Move[] optimalMoves, out AgentState agentStartState, out int start)
{
// Run a single source shortest path algorithm from the _goal_ of the agent
shortestPathLengths = new int[this.numLocations];
optimalMoves = new Move[this.numLocations];
for (int i = 0; i < numLocations; i++)
{
shortestPathLengths[i] = -1;
}
var openlist = new Queue <AgentState>();
// Create initial state
agentStartState = this.agents[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 = cardinality[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);
}
}
}
}
start = this.GetCardinality(agentStartState.lastMove);
if (shortestPathLengths[start] == -1)
{
throw new Exception($"Unsolvable instance! Agent {agentId} cannot reach its goal");
// Note instances can still be unsolvable if this isn't reached. E.g. this corridor:
// s1-g2-g1-s2
}
}
/// <summary>
/// Imports a problem instance from a given file
/// </summary>
/// <param name="filePath"></param>
/// <param name="mapFilePath"></param>
/// <returns></returns>
public static ProblemInstance Import(string filePath, string mapFilePath = null)
{
if (filePath.EndsWith(".agents"))
{
string fileNameWithoutExtension = Path.GetFileNameWithoutExtension(filePath);
int instanceId = 0;
try
{
instanceId = int.Parse(filePath.Split('_').Last());
}
catch (Exception) {}
string mapfileNameWithoutExtension;
if (mapFilePath == null)
{
mapfileNameWithoutExtension = fileNameWithoutExtension.Substring(0, length: fileNameWithoutExtension.LastIndexOf('_')) + ".map"; // Passing a length parameter is like specifying a non-inclusive end index
mapFilePath = Path.Combine(Path.GetDirectoryName(filePath), "..", "maps", mapfileNameWithoutExtension);
}
else
{
mapfileNameWithoutExtension = Path.GetFileNameWithoutExtension(mapFilePath);
}
bool[][] grid = readMapFile(mapFilePath);
string line;
string[] lineParts;
AgentState[] states;
using (TextReader input = new StreamReader(filePath))
{
// Read the number of agents
line = input.ReadLine();
int numOfAgents = int.Parse(line);
// Read the agents' start and goal states
states = new AgentState[numOfAgents];
AgentState state;
Agent agent;
int goalX;
int goalY;
int startX;
int startY;
for (int i = 0; i < numOfAgents; i++)
{
line = input.ReadLine();
lineParts = line.Split(EXPORT_DELIMITER);
goalX = int.Parse(lineParts[0]);
goalY = int.Parse(lineParts[1]);
startX = int.Parse(lineParts[2]);
startY = int.Parse(lineParts[3]);
agent = new Agent(goalX, goalY, i);
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] = mapfileNameWithoutExtension;
instance.parameters[ProblemInstance.INSTANCE_NAME_KEY] = fileNameWithoutExtension + ".agents";
instance.ComputeSingleAgentShortestPaths();
return(instance);
}
else if (filePath.EndsWith(".scen"))
{
string fileNameWithoutExtension = Path.GetFileNameWithoutExtension(filePath);
int instanceId = int.Parse(fileNameWithoutExtension.Split('-').Last());
string mapfileName = fileNameWithoutExtension.Substring(0, length: fileNameWithoutExtension.LastIndexOf('-')); // Passing a length parameter is like specifying a non-inclusive end index
if (mapFilePath == null)
{
mapFilePath = Path.Combine(Path.GetDirectoryName(filePath), "..", "..", "..", "maps", mapfileName);
}
bool[][] grid = readMapFile(mapFilePath);
string line;
string[] lineParts;
List <AgentState> stateList = new List <AgentState>();
using (TextReader input = new StreamReader(filePath))
{
// Read the format version number
line = input.ReadLine();
lineParts = line.Split(' ');
Trace.Assert(lineParts[0].Equals("version"));
int version = int.Parse(lineParts[1]);
Trace.Assert(version == 1, "Only version 1 is currently supported");
// Read the agents' start and goal states
AgentState state;
Agent agent;
int agentNum = 0;
int block;
int goalX;
int goalY;
int startX;
int startY;
string mapFileName;
int mapRows;
int mapCols;
double optimalCost; // Assuming diagonal moves are allowed and cost sqrt(2)
while (true)
{
line = input.ReadLine();
if (string.IsNullOrWhiteSpace(line))
{
break;
}
lineParts = line.Split('\t');
block = int.Parse(lineParts[0]);
mapFileName = lineParts[1];
mapRows = int.Parse(lineParts[2]);
Trace.Assert(mapRows == grid.GetLength(0));
mapCols = int.Parse(lineParts[3]);
Trace.Assert(mapCols == grid.GetLength(1));
startY = int.Parse(lineParts[4]);
startX = int.Parse(lineParts[5]);
goalY = int.Parse(lineParts[6]);
goalX = int.Parse(lineParts[7]);
optimalCost = double.Parse(lineParts[8]);
agent = new Agent(goalX, goalY, agentNum);
state = new AgentState(startX, startY, agent);
stateList.Add(state);
agentNum++;
}
}
// Generate the problem instance
ProblemInstance instance = new ProblemInstance();
instance.Init(stateList.ToArray(), grid);
instance.instanceId = instanceId;
instance.parameters[ProblemInstance.GRID_NAME_KEY] = mapfileName;
instance.parameters[ProblemInstance.INSTANCE_NAME_KEY] = fileNameWithoutExtension + ".scen";
instance.ComputeSingleAgentShortestPaths();
return(instance);
}
else // Combined map and scenario, no suffix
{
using (TextReader input = new StreamReader(filePath))
{
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();
}
// First/second line is Grid:
Trace.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][];
// Read grid
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[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();
Trace.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.parameters[ProblemInstance.INSTANCE_NAME_KEY] = Path.GetFileName(filePath);
instance.ComputeSingleAgentShortestPaths();
return(instance);
}
}
}
/// <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...");
Stopwatch watch = Stopwatch.StartNew();
double startTime = watch.Elapsed.TotalMilliseconds;
//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.numLocations];
var optimalMoves = new Move[this.numLocations];
for (int i = 0; i < numLocations; i++)
{
shortestPathLengths[i] = -1;
}
var openlist = new Queue <AgentState>();
// Create initial state
var agentStartState = this.agents[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 = cardinality[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($"Unsolvable instance! Agent {agentId} cannot reach its goal");
// Note instances can still be unsolvable if this isn't reached. E.g. this corridor:
// s1-g2-g1-s2
}
this.singleAgentOptimalCosts[agentId] = shortestPathLengths;
this.singleAgentOptimalMoves[agentId] = optimalMoves;
}
double endTime = watch.Elapsed.TotalMilliseconds;
this.shortestPathComputeTime = endTime - startTime;
}