ObstacleGrid CreateProperObstaclePattern(ObstacleGrid obstacle)
{
var obs = obstacle;
if (RoadWidth > obs.Grid.Count)
{
var emptyColumn = new Column(obs.Grid[0].column.Count);
var emptyColumnCount = RoadWidth - obs.Grid.Count;
for (int i = 0; i < emptyColumnCount * .5f; i++)
{
obs.Grid.Insert(0, emptyColumn);
}
while (RoadWidth > obs.Grid.Count)
{
obs.Grid.Insert(obs.Grid.Count, emptyColumn);
}
}
else if (RoadWidth < obs.Grid.Count)
{
for (int i = 0; i < (obs.Grid.Count - RoadWidth) * .5f; i++)
{
obstacle.Grid.RemoveAt(0);
}
while (RoadWidth < obs.Grid.Count)
{
obs.Grid.RemoveAt(obs.Grid.Count - 1);
}
}
return(obs);
}
public GVDKarla(ObstacleGrid grid)
{
this.grid = grid;
open = new IntervalHeap<GridCellValue>();
ties = new LinkedList<GridCell>();
dist = new float[grid.NumColumns, grid.NumRows];
distNew = new float[grid.NumColumns, grid.NumRows];
parent = new GridCell[grid.NumColumns, grid.NumRows];
tie = new GridCell[grid.NumColumns, grid.NumRows];
obst = new int[grid.NumColumns, grid.NumRows];
valid = new HashSet<int>();
voro = new bool[grid.NumColumns, grid.NumRows];
for (int c = grid.NumColumns - 1; c >= 0; c--)
for (int r = grid.NumRows - 1; r >= 0; r--)
{
dist[c, r] = float.PositiveInfinity;
distNew[c, r] = float.PositiveInfinity;
parent[c, r] = GridCell.Unknown;
tie[c, r] = GridCell.Unknown;
obst[c, r] = -1;
voro[c, r] = false;
}
}
ObstacleGrid MapAllRoad(int difficulty)
{
var Map = new ObstacleGrid(RoadWidth, FirstObstacleStartPosition);
var obstacle = GetRandomObstacle();
Map.AddSameSizeRowGrid(CreateProperObstaclePattern(obstacle).Grid);
var currentColumn = FirstObstacleStartPosition + obstacle.Grid[0].column.Count + 1;
while (currentColumn < RoadLength - 5)
{
var ObstacleChance = difficulty * ObstacleChanceConstant;
var putObstacle = Random.Range(0, 1f) < ObstacleChance;
if (putObstacle)
{
obstacle = GetRandomObstacle();
Map.AddSameSizeRowGrid(CreateProperObstaclePattern(obstacle).Grid);
currentColumn += obstacle.Grid[0].column.Count;
}
Map.AddEmptyRow();
currentColumn++;
}
while (Map.Grid[0].column.Count < RoadLength)
{
Map.AddEmptyRow();
}
return(Map);
}
public ObstacleRelaxed(ObstacleGrid grid)
{
this.grid = grid;
}
public CombinedHeuristic(ObstacleGrid grid)
{
one = new NonholonomicRelaxed(grid);
two = new ObstacleRelaxed(grid);
}
public NonholonomicRelaxed(ObstacleGrid grid)
{
this.grid = grid;
heuristic = new float[grid.NumColumns, grid.NumRows];
unit = grid.Resolution * discount; // factor to discount the suboptimality of 8-neighbor paths
}
public void InitSim(bool autoStart, bool smoothingOn, Mission mission)
{
gridDone = false;
pathDone = false;
pathSmoothDone = false;
pathSearching = false;
pathSearchingDone = false;
autoDrive = true;
run = false;
isCollided = false;
currentControls = new CarControls(0f, 0f, 0f);
if (camera == null)
{
camera = new Camera(MathHelper.PiOver4, GraphicsDevice.Viewport.AspectRatio, 0.1f, 1000f);
camera.Position = new Vector3(75f, 75f, 180f);
}
startPose = mission.Start;
goalPose = mission.Goal;
HybridAStar.Epsilon = mission.AStarEpsilon;
HybridAStar.GridResolution = mission.AStarGridResolution;
HybridAStar.SafetyFactor = 1.5f;
HybridAStar.Reset();
car = new Car(world, startPose);
grid = new ObstacleGrid(world, mission.Environment);
car.Body.OnCollision += new OnCollisionEventHandler(OnCollision);
gridDone = false;
pathDone = false;
pathSearchingDone = false;
bg = new Thread(() =>
{
DateTime now = DateTime.Now;
grid.BuildGVD();
console.WriteLine("GVD Generation Time: " + Math.Round((DateTime.Now - now).TotalMilliseconds) + " ms");
gridDone = true;
now = DateTime.Now;
pathSearching = true;
astar = HybridAStar.FindPath(grid, startPose, goalPose);
TimeSpan astarTime = DateTime.Now - now;
poses = astar.Path;
pathSearching = false;
pathSearchingDone = true;
if (astar.Path.Count > 0)
{
pathDone = true;
}
now = DateTime.Now;
if (smoothingOn)
{
smoothedPath = Smoother.Smooth(astar.Path, grid);
}
else
{
smoothedPath = astar.Path;
}
TimeSpan smoothingTime = DateTime.Now - now;
int numUnsafe = Smoother.UnsafeIndices != null ? Smoother.UnsafeIndices.Count : 0;
console.WriteLine("A*: Total Planning Time: " + Math.Round((astarTime + smoothingTime).TotalMilliseconds) + " ms");
console.WriteLine(" Heuristic Time: " + Math.Round(astar.HeuristicInitTime.TotalMilliseconds) + " ms");
console.WriteLine(" Searching Time: " + Math.Round((astarTime - astar.HeuristicInitTime).TotalMilliseconds) + " ms");
console.WriteLine(" Smoothing Time: " + Math.Round(smoothingTime.TotalMilliseconds) + " ms (" + Smoother.NumIterations + " iterations, " + Smoother.Change + "m, " + numUnsafe + " unsafe points)");
console.WriteLine(" " + astar.Discovered.Count + " nodes discovered");
console.WriteLine(" " + astar.Expanded.Count + " nodes expanded");
controller = new StanleyFSMController(smoothedPath, goalPose);
pathSmoothDone = true;
if (autoStart)
{
run = true;
}
});
bg.IsBackground = true;
bg.Priority = ThreadPriority.Lowest;
bg.Start();
}
public static HybridAStarResults FindPath(ObstacleGrid aGrid, Pose start, Pose aGoal)
{
grid = aGrid;
goal = aGoal;
heuristicUpdated = false;
heuristicBitmap = null;
IntervalHeap <Node> open = new IntervalHeap <Node>();
//heuristic = new EuclideanHeuristic();
//heuristic = new ObstacleRelaxed(grid);
heuristic = new NonholonomicRelaxed(grid);
//heuristic = new CombinedHeuristic(grid);
DateTime now = DateTime.Now;
heuristic.Update(goal);
heuristicUpdated = true;
TimeSpan heuristicInitTime = DateTime.Now - now;
Expanded = new LinkedList <Node>();
LinkedList <Node> discovered = new LinkedList <Node>();
int numCols = (int)Math.Ceiling(grid.NumColumns * grid.Resolution / GridResolution);
int numRows = (int)Math.Ceiling(grid.NumRows * grid.Resolution / GridResolution);
CellState?[, , ,] cells = new CellState?[numCols, numRows, numOrientations, 2];
Cell startCell = poseToCell(start, 0);
IPriorityQueueHandle <Node> startHandle = null;
float heuristicValue = heuristic.GetHeuristicValue(start, goal);
Node startNode = new Node(startCell, new ReedsSheppAction(Steer.Straight, Gear.Forward, 0f), start, 0, Epsilon * heuristicValue, null);
open.Add(ref startHandle, startNode);
discovered.Add(startNode);
cells[startCell.c, startCell.r, startCell.o, startCell.rev] = new CellState(start, Epsilon * heuristicValue, startHandle);
while (!open.IsEmpty)
{
Node n = open.DeleteMin();
lock (Expanded)
{
Expanded.Add(n);
}
CellState?c = cells[n.cell.c, n.cell.r, n.cell.o, n.cell.rev];
if (c.Value.continuousPose == goal)
{
return(new HybridAStarResults(reconstructPath(n), Expanded, discovered, heuristicInitTime));
}
float dt = GridResolution / VehicleModel.SlowVelocity;
//GridCell nodeCell = grid.PointToCellPosition(c.Value.continuousPose.Position);
//float dt = MathHelper.Max(GridResolution, 0.5f * (grid.GVD.GetObstacleDistance(nodeCell) + grid.GVD.GetVoronoiDistance(nodeCell)));
//dt /= VehicleModel.SlowVelocity;
for (int g = 0; g < VehicleModel.NumGears; g++)
{
Gear gear = (Gear)g;
foreach (Node child in getChildren(c.Value.continuousPose, gear, dt, goal))
{
int rev = gear == Gear.Forward ? 0 : 1;
float tentativeg;
if (child.actionSet == null)
{
tentativeg = n.g + pathCost(n.pose, n.cell.rev, child.pose, rev, dt);
}
else
{
tentativeg = n.g + child.actionSet.CalculateCost(VehicleModel.TurnRadius, reverseFactor, switchPenalty);
}
float tentativef = tentativeg + Epsilon * heuristic.GetHeuristicValue(child.pose, goal);
child.cell = poseToCell(child.pose, rev);
CellState?currentCell = cells[child.cell.c, child.cell.r, child.cell.o, child.cell.rev];
if (!currentCell.HasValue)
{
IPriorityQueueHandle <Node> childHandle = null;
child.g = tentativeg;
child.f = tentativef;
child.from = n;
open.Add(ref childHandle, child);
discovered.Add(child);
cells[child.cell.c, child.cell.r, child.cell.o, child.cell.rev] = new CellState(child.pose, tentativef, childHandle);
}
else if (tentativef < currentCell.Value.f)
{
IPriorityQueueHandle <Node> currentHandle = currentCell.Value.handle;
child.g = tentativeg;
child.f = tentativef;
child.from = n;
Node temp;
if (open.Find(currentHandle, out temp))
{
open[currentHandle] = child;
}
else
{
open.Add(ref currentHandle, child);
}
discovered.Add(child);
cells[child.cell.c, child.cell.r, child.cell.o, child.cell.rev] = new CellState(child.pose, tentativef, currentHandle);
}
}
}
if (Delay > 0)
{
Thread.Sleep(Delay);
}
}
return(new HybridAStarResults(new ArrayList <Pose>(), Expanded, discovered, heuristicInitTime));
}
public static ArrayList<Pose> Smooth(ArrayList<Pose> path, ObstacleGrid grid)
{
return Smooth(path, grid, null);
}
public void InitSim(bool autoStart, bool smoothingOn, Mission mission)
{
gridDone = false;
pathDone = false;
pathSmoothDone = false;
pathSearching = false;
pathSearchingDone = false;
autoDrive = true;
run = false;
isCollided = false;
currentControls = new CarControls(0f, 0f, 0f);
if (camera == null)
{
camera = new Camera(MathHelper.PiOver4, GraphicsDevice.Viewport.AspectRatio, 0.1f, 1000f);
camera.Position = new Vector3(75f, 75f, 180f);
}
startPose = mission.Start;
goalPose = mission.Goal;
HybridAStar.Epsilon = mission.AStarEpsilon;
HybridAStar.GridResolution = mission.AStarGridResolution;
HybridAStar.SafetyFactor = 1.5f;
HybridAStar.Reset();
car = new Car(world, startPose);
grid = new ObstacleGrid(world, mission.Environment);
car.Body.OnCollision += new OnCollisionEventHandler(OnCollision);
gridDone = false;
pathDone = false;
pathSearchingDone = false;
bg = new Thread(() =>
{
DateTime now = DateTime.Now;
grid.BuildGVD();
console.WriteLine("GVD Generation Time: " + Math.Round((DateTime.Now - now).TotalMilliseconds) + " ms");
gridDone = true;
now = DateTime.Now;
pathSearching = true;
astar = HybridAStar.FindPath(grid, startPose, goalPose);
TimeSpan astarTime = DateTime.Now - now;
poses = astar.Path;
pathSearching = false;
pathSearchingDone = true;
if (astar.Path.Count > 0)
pathDone = true;
now = DateTime.Now;
if (smoothingOn)
smoothedPath = Smoother.Smooth(astar.Path, grid);
else
smoothedPath = astar.Path;
TimeSpan smoothingTime = DateTime.Now - now;
int numUnsafe = Smoother.UnsafeIndices != null ? Smoother.UnsafeIndices.Count : 0;
console.WriteLine("A*: Total Planning Time: " + Math.Round((astarTime + smoothingTime).TotalMilliseconds) + " ms");
console.WriteLine(" Heuristic Time: " + Math.Round(astar.HeuristicInitTime.TotalMilliseconds) + " ms");
console.WriteLine(" Searching Time: " + Math.Round((astarTime - astar.HeuristicInitTime).TotalMilliseconds) + " ms");
console.WriteLine(" Smoothing Time: " + Math.Round(smoothingTime.TotalMilliseconds) + " ms (" + Smoother.NumIterations + " iterations, " + Smoother.Change + "m, " + numUnsafe + " unsafe points)");
console.WriteLine(" " + astar.Discovered.Count + " nodes discovered");
console.WriteLine(" " + astar.Expanded.Count + " nodes expanded");
controller = new StanleyFSMController(smoothedPath, goalPose);
pathSmoothDone = true;
if (autoStart)
run = true;
});
bg.IsBackground = true;
bg.Priority = ThreadPriority.Lowest;
bg.Start();
}
private static HashSet<int> checkPath(ArrayList<Pose> path, ObstacleGrid grid)
{
HashSet<int> unsafeIndices = new HashSet<int>();
int count = path.Count;
for (int i = 2; i < count - 2; i++)
{
if (path[i - 2].Gear != path[i - 1].Gear) continue;
if (path[i - 1].Gear != path[i].Gear) continue;
if (path[i].Gear != path[i + 1].Gear) continue;
if (path[i + 1].Gear != path[i + 2].Gear) continue;
Vector2 currPos = path[i].Position;
Vector2 prevPos = path[i - 1].Position;
Vector2 displacement = 0.25f * (currPos - prevPos) + 0.75f * (currPos - currPos);
float pathOrientation = (float)Math.Atan2(displacement.Y, displacement.X);
if (!grid.IsSafe(new Pose(currPos, pathOrientation), 1.1f))
{
unsafeIndices.Add(i);
unsafeIndices.Add(i - 1);
}
}
return unsafeIndices;
}
public static ArrayList<Pose> Smooth(ArrayList<Pose> path, ObstacleGrid grid, HashSet<int> unsafeIndices)
{
UnsafeIndices = Anchoring ? unsafeIndices : null;
if (path.Count < 5)
{
ArrayList<Pose> thepath = new ArrayList<Pose>();
thepath.AddAll(path);
return thepath;
}
int num = currentCount = path.Count;
currentPath = new Pose[num];
for (int i = 0; i < num; i++)
currentPath[i] = path[i];
int count = 0;
float change = Tolerance;
while (/*change >= Tolerance && */count < MaxIterations)
{
float cdmax2 = CollisionDmax * CollisionDmax;
change = 0f;
for (int i = 2; i < num - 2; i++)
{
// Keep this point fixed if it is or is adjacent to a cusp point
if (currentPath[i - 2].Gear != currentPath[i - 1].Gear) continue;
if (currentPath[i - 1].Gear != currentPath[i].Gear) continue;
if (currentPath[i].Gear != currentPath[i + 1].Gear) continue;
if (currentPath[i + 1].Gear != currentPath[i + 2].Gear) continue;
// Keep this point fixed if it is unsafe
if (Anchoring && unsafeIndices != null && unsafeIndices.Contains(i))
continue;
Vector2 curr = currentPath[i].Position;
Vector2 correction = Vector2.Zero;
// Original path term
correction += PathWeight * (path[i].Position - curr);
if (float.IsNaN(correction.X) || float.IsNaN(correction.Y))
{
float noop;
}
if (grid.IsPointInGrid(curr))
{
// Collision term
Vector2 closestObstacleVec = curr - grid.CellPositionToPoint(grid.GVD.GetNearestObstacle(grid.PointToCellPosition(curr)));
float obstDist = closestObstacleVec.Length();
if (obstDist < CollisionDmax)
correction -= CollisionWeight * (obstDist - CollisionDmax) * closestObstacleVec / obstDist;
if (float.IsNaN(correction.X) || float.IsNaN(correction.Y))
{
float noop;
}
// Voronoi term
if (obstDist < VoronoiDmax && VoronoiWeight > 0f)
{
Vector2 closestVoronoiVec = curr - grid.CellPositionToPoint(grid.GVD.GetNearestVoronoiEdge(grid.PointToCellPosition(curr)));
float voroDist = closestVoronoiVec.Length();
if (voroDist > 0f)
{
float alphaplusdo = grid.GVD.AlphaActual + obstDist;
float dominusdmax = obstDist - VoronoiDmax;
float doplusdv = obstDist + voroDist;
float dmaxsquared = VoronoiDmax * VoronoiDmax;
float pvdv = (grid.GVD.AlphaActual / alphaplusdo) * (dominusdmax * dominusdmax / dmaxsquared) * (obstDist / (doplusdv * doplusdv));
float pvdo = (grid.GVD.AlphaActual / alphaplusdo) * (voroDist / doplusdv) * (dominusdmax / dmaxsquared) * (-dominusdmax / alphaplusdo - dominusdmax / doplusdv + 2);
correction -= VoronoiWeight * (pvdo * closestObstacleVec / obstDist + pvdv * closestVoronoiVec / voroDist);
if (float.IsNaN(correction.X) || float.IsNaN(correction.Y))
{
float noop;
}
}
}
}
// Smoothing term
correction -= SmoothWeight * (currentPath[i - 2].Position - 4 * currentPath[i - 1].Position + 6 * curr - 4 * currentPath[i + 1].Position + currentPath[i + 2].Position);
if (float.IsNaN(correction.X) || float.IsNaN(correction.Y))
{
float noop;
}
// Curvature term
correction -= CurvatureWeight * calcCurvatureTerm(currentPath[i - 1].Position, currentPath[i].Position, currentPath[i + 1].Position);
if (float.IsNaN(correction.X) || float.IsNaN(correction.Y))
{
calcCurvatureTerm(currentPath[i - 1].Position, currentPath[i].Position, currentPath[i + 1].Position);
}
correction /= CollisionWeight + SmoothWeight + VoronoiWeight + PathWeight + CurvatureWeight;
currentPath[i].Position = curr + Alpha * correction;
change += correction.Length();
}
count++;
if (Delay > 0)
Thread.Sleep(Delay);
}
NumIterations = count;
Change = change;
ArrayList<Pose> thenewpath = new ArrayList<Pose>();
thenewpath.AddAll(currentPath);
if (Anchoring && unsafeIndices == null)
{
HashSet<int> newUnsafes = checkPath(thenewpath, grid);
if (newUnsafes.Count > 0)
thenewpath = Smooth(path, grid, newUnsafes);
}
foreach (Pose p in thenewpath)
{
if (float.IsNaN(p.Position.X) || float.IsNaN(p.Position.Y))
{
float noop;
}
}
return thenewpath;
}
public CombinedHeuristic(ObstacleGrid grid)
{
one = new NonholonomicRelaxed(grid);
two = new ObstacleRelaxed(grid);
}
public ObstacleRelaxed(ObstacleGrid grid)
{
this.grid = grid;
}
public NonholonomicRelaxed(ObstacleGrid grid)
{
this.grid = grid;
heuristic = new float[grid.NumColumns, grid.NumRows];
unit = grid.Resolution * discount; // factor to discount the suboptimality of 8-neighbor paths
}
