static void MoveRobotToTarget(Pos targetPos) { CreateVirtualWall(false); List <Direction> dirs = FastestPath.AStarSearch(currentPos, targetPos, grids, false); if (null != dirs) { foreach (Direction nextMove in dirs) { // Debug.Log ("TargetPos = " + targetPos.ToString () + ", " + // "currentPos = " + currentPos.ToString () + ", " + // "currentDir = " + currentDir + ", " + // "next move is: " + nextMove); MoveByNextDirection(currentDir, nextMove); //_currExploredCount = CountExploredGrids(); //if (_currExploredCount - _lastExploredCount >= 3) { // AdjustForRightHandWall(); // Debug.Log("Exit FP when having new explored, robot at: " + currentPos.x + ", " + currentPos.y); // return; //} //_lastExploredCount = _currExploredCount; } } Debug.Log("Exit FP, robot at: " + currentPos.x + ", " + currentPos.y); }
public int CalculateTravelTime(FastestPath fastestPath) { var travelStartTime = fastestPath.Path.First(p => p.IsTransfer == false).StartDateTime; var travelEndTime = fastestPath.Path.Last(p => p.IsTransfer == false).EndDateTime; return((int)Math.Abs((travelEndTime - travelStartTime).TotalMinutes)); }
public FastestPathDescription WriteDescription (SearchInput search, FastestPath fastestPath) { var description = new FastestPathDescription(); if (IsTheFastestPathByFoot(fastestPath)) { description.DescriptionRows.Add(WriteTheSameStopsCommunicate()); return(description); } int travelTime = _timeCalculator.CalculateTravelTime(fastestPath); description.DescriptionRows.Add( WriteHeader(search.StartStop, search.DestinationStop, travelTime)); foreach (var connection in fastestPath.FlattenPath) { if (!connection.IsTransfer) { description.DescriptionRows = description.DescriptionRows.Concat(WriteLine(connection)).ToList(); } else { description.DescriptionRows.Add(WriteTransfer(connection)); } } description.DescriptionRows = description.DescriptionRows.Reverse().ToList(); return(description); }
private bool IsTheFastestPathByFoot(FastestPath fastestPath) { return (fastestPath.Path.Count() == 1 && (fastestPath.Path.First().SourceStop.Stop.Id == fastestPath.Path.First().DestinationStop.Stop.Id || fastestPath.Path.First().IsTransfer)); }
public void RunFastestPath() { if (!hasCompleteMap) { Debug.LogError("No active complete map, can't do Fastest Path! "); return; } inFastestPath = true; InitRobot(); currentDir = Direction.EAST; // hardcoded List <Direction> solution; if (waypoint.x != -1) // if has waypoint { solution = FastestPath.AStarSearch(new Pos(1, 1), waypoint, grids, diagonal); if (solution.Count < 1) { Debug.Log("Cannot find a path to waypoint, aborting."); inFastestPath = false; return; } Direction initialDir = currentDir; currentDir = solution[solution.Count - 1]; solution.AddRange(FastestPath.AStarSearch(waypoint, new Pos(13, 18), grids, diagonal)); currentDir = initialDir; } else { solution = FastestPath.AStarSearch(grids, diagonal); } foreach (Direction d in solution) { Debug.Log(d); } List <Instruction> li = ConvertToInstructionList(solution, diagonal); try { SendFastestPathInstructions(li); } catch { } // for Unity display // StartCoroutine() prevents this from being static! StartCoroutine("ShowPath", li); }
private static void UpdateNotYetTaken() { if (!toggleImageFlag) { return; } for (int i = 0; i < 15; i++) { for (int j = 0; j < 20; j++) { if (grids[i, j].gs == GridStatus.WALL) { foreach (Direction dir in directionsForExplore) { if (CheckNeighbourGrid(i + DirectionToVector(dir).x, j + DirectionToVector(dir).y)) { notYetTakenSurfaces.Add(PosXYToMsgFormat(i, j) + dir.ToString()); } } } } } string lastStr = ""; int lastShortest = -1; int x = -1; string lastDir = ""; int y = -1; string direction = ""; string lastSur = ""; bool hasUnvisited = false; do { x = -1; y = -1; lastStr = ""; lastDir = ""; lastShortest = -1; hasUnvisited = false; foreach (var sur in notYetTakenSurfaces) { //if (photosTaken == 5) break; if (!visitedEdges.Contains(sur)) { hasUnvisited = true; string surface = CheckValidSurface(sur); if (surface != "invalid") { x = ParseStringToInteger(surface.Substring(0, 2)); y = ParseStringToInteger(surface.Substring(2, 2)); direction = surface.Substring(4); // direction if (lastStr == "" && lastShortest == -1) { lastSur = sur; lastDir = direction; lastStr = surface; lastShortest = FastestPath.AStarSearch(new Pos(1, 1), new Pos(x, y), grids, false).Count; } else if (FastestPath.AStarSearch(new Pos(1, 1), new Pos(x, y), grids, false).Count > lastShortest) { lastSur = sur; lastStr = surface; lastDir = direction; lastShortest = FastestPath.AStarSearch(new Pos(1, 1), new Pos(x, y), grids, false).Count; } } else { visitedEdges.Add(sur); Debug.Log("No Valid location for surface: " + sur); } } } if (hasUnvisited) { Debug.Log("Going to x: " + lastStr.Substring(0, 2) + " ,y: " + lastStr.Substring(2, 2) + " Facing: " + lastStr.Substring(4) + " to capture surface: " + lastSur); MoveRobotToTarget(new Pos(ParseStringToInteger(lastStr.Substring(0, 2)), ParseStringToInteger(lastStr.Substring(2, 2)))); visitedEdges.Add(lastSur); // Change direction if needed foreach (Direction dir in directionsForExplore) { if (dir.ToString() == lastStr.Substring(4)) { if (currentDir != dir) { TurnToDirection(dir); } } } } } while (hasUnvisited); MoveRobotToTarget(new Pos(1, 1)); }