コード例 #1
0
        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);
        }
コード例 #2
0
ファイル: AgentState.cs プロジェクト: triplez23/mapf-solvers
        /// <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);
        }
コード例 #3
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++;
             }
         }
     }
 }
コード例 #4
0
        /// <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));
        }
コード例 #5
0
ファイル: AgentState.cs プロジェクト: triplez23/mapf-solvers
        /// <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
            }
        }
コード例 #6
0
        /// <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));
        }
コード例 #7
0
        /// <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);
        }
コード例 #8
0
        /// <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);
                }
            }
        }
コード例 #9
0
        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);
        }
コード例 #10
0
        /// <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]);
        }
コード例 #11
0
        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
            }
        }
コード例 #12
0
        /// <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);
                }
            }
        }
コード例 #13
0
        /// <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;
        }