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));
}
