public bool FindPath(ref RRTNode originalRoot, Vector2 goalPoint, List <Polygon> obstacles, out RRTNode goal)
{
RRTNode root = new RRTNode(originalRoot.State);
double distance = goalPoint.DistanceTo(root.State.Pose.ToVector2());
randomSampleRadius = distance + 15.0;
timeStep = rand.NextDouble();
//vSigma = rand.NextDouble() * 5.0;
numSlice = (int)Math.Round(timeStep / 0.2);
//if (distance < 2)
// timeStep = 0.3;
//else if (distance > 4)
// timeStep = 1.0;
//else
// timeStep = 0.5;
//numSlice = (int)Math.Round(timeStep / 0.1);
Stopwatch randomGenerationTimer = new Stopwatch();
Stopwatch extendingTimer = new Stopwatch();
Stopwatch closestSearchTimer = new Stopwatch();
List <Double> randomTime = new List <double>();
List <Double> extendingTime = new List <double>();
List <Double> closestSearchTime = new List <double>();
bool foundPath = false;
goal = null; //not found yet!
//RRT is divided into the following steps:
//0) assume the root node is the first node
//1) randomly select a sample point in space centered around our robot within some fixed distance. Every 20th can be the goal.
//2) select the closest node to the sampled point in the existing tree based on xy distance
//3) generate a control input that drives towards the sample point also biased with our initial control inputs
//3a) -Biasing Details:
// Select Velocity: Normal Distribution with mean = closest node velocity and sigma = SigmaVelocity
// Select Turn Rate:
// Apply the following heuristic: mean = (atan2(yf-yi,xf-xi) - thetaInit)/(delT)
// sigma = SigmaTurnRate
//4) Divide the total RRT timestep into smaller sub-steps
//4a) calculate the trajectory at one substep given the control inputs and closest node initial conditions
//4b) check at each the linear path between the initial and simulation end does not intersect a polygon
//4c) if intersects, terminate and go to 1.
//4d) if not intersects
//4da) if last substep, add the results of this simulation to the closest node as a child
//4db) else simulate the next subtime step by going to 4a
//5) Check if the new node added is within some tolerance of the goal node. If so, mark node as goal and you're done! Else, Goto 1.
//----------------------------------------------------------------------------------------------------------------------------------//
// Declare variables
int sampleCount = 0; // counter for sample to be biased every 20th time
int iterationCount = 0; // counter for termination
kdTree = new KDTree(2);
kdTree.insert(RRTNode.ToKey(root.State.Pose.x, root.State.Pose.y), root);
// 0) assume root note is the first node
while (!foundPath)
//for (int i = 0; i < 1000; i++)
{
//--- Termination ---//
iterationCount++;
if (iterationCount > terminationCount)
{
Console.WriteLine("//-----------------------------------------------------------------------//");
Console.WriteLine("Random generation average time: " + (randomTime.Sum() / randomTime.Count) + " ms | total time: " + randomTime.Sum() + " | iteration: " + randomTime.Count);
Console.WriteLine("Searching closest node average time: " + (closestSearchTime.Sum() / closestSearchTime.Count) + " ms | total time: " + closestSearchTime.Sum() + " | iteration: " + closestSearchTime.Count);
Console.WriteLine("Extending average time: " + (extendingTime.Sum() / extendingTime.Count) + " ms | total time: " + extendingTime.Sum() + " | iteration: " + extendingTime.Count);
// Benchmark output
Console.WriteLine("|--Simulation average time: " + (simulationTime.Sum() / simulationTime.Count) + " ms | total time: " + simulationTime.Sum() + " | iteration: " + simulationTime.Count);
Console.WriteLine("|--Obstacle checking average time: " + (obstacleTime.Sum() / obstacleTime.Count) + " ms | total time: " + obstacleTime.Sum() + " | iteration: " + obstacleTime.Count);
Console.WriteLine("//-----------------------------------------------------------------------//");
simulationTime.Clear();
obstacleTime.Clear();
return(false);
}
//-------------------//
// 1) randomly select a sample point in space centered around our robot within some fixed distance.
int actualNumNodesToExtend = numNodesToExtend;
Vector2 samplePoint;
if (sampleCount < goalPointSamplingRate)
{
if (rand.NextDouble() > chanceToSampleRoot)
{
randomGenerationTimer.Start();
//double randomX = root.State.Pose.x + (rand.NextDouble() - .5) * randomSampleRadius * 2.0;
//double randomY = root.State.Pose.y + (rand.NextDouble() - .5) * randomSampleRadius * 2.0;
double randomX = goalPoint.X + (rand.NextDouble() - .5) * randomSampleRadius * 2.0;
double randomY = goalPoint.Y + (rand.NextDouble() - .5) * randomSampleRadius * 2.0;
samplePoint = new Vector2(randomX, randomY);
randomTime.Add(randomGenerationTimer.ElapsedMilliseconds);
randomGenerationTimer.Reset();
}
else
{
samplePoint = root.Point;
actualNumNodesToExtend = 1;
}
sampleCount++;
}
else
{
samplePoint = goalPoint;
sampleCount = 0;
}
closestSearchTimer.Start();
// 2) select the closest node to the sampled point in the existing tree based on xy distance
//List<RRTNode> closestNodes = root.FindNClosestNodeInTree(samplePoint, actualNumNodesToExtend);
List <RRTNode> closestNodes = root.FindNClosestNodeInTreeKDTREE(3, samplePoint, kdTree);
closestSearchTime.Add(closestSearchTimer.ElapsedMilliseconds);
closestSearchTimer.Reset();
extendingTimer.Start();
foreach (RRTNode closeNode in closestNodes)
{
RRTNode newNode = ExtendNode(ref goalPoint, obstacles, ref goal, ref foundPath, ref samplePoint, closeNode, rand);
if (newNode != null)
{
kdTree.insert(RRTNode.ToKey(newNode.State.Pose.x, newNode.State.Pose.y), newNode);
}
}
extendingTime.Add(extendingTimer.ElapsedMilliseconds);
extendingTimer.Reset();
}
Console.WriteLine("//-----------------------------------------------------------------------//");
Console.WriteLine("Random generation average time: " + (randomTime.Sum() / randomTime.Count) + " ms | total time: " + randomTime.Sum() + " | iteration: " + randomTime.Count);
Console.WriteLine("Searching closest node average time: " + (closestSearchTime.Sum() / closestSearchTime.Count) + " ms | total time: " + closestSearchTime.Sum() + " | iteration: " + closestSearchTime.Count);
Console.WriteLine("Extending average time: " + (extendingTime.Sum() / extendingTime.Count) + " ms | total time: " + extendingTime.Sum() + " | iteration: " + extendingTime.Count);
// Benchmark output
Console.WriteLine("|--Simulation average time: " + (simulationTime.Sum() / simulationTime.Count) + " ms | total time: " + simulationTime.Sum() + " | iteration: " + simulationTime.Count);
Console.WriteLine("|--Obstacle checking average time: " + (obstacleTime.Sum() / obstacleTime.Count) + " ms | total time: " + obstacleTime.Sum() + " | iteration: " + obstacleTime.Count);
Console.WriteLine("//-----------------------------------------------------------------------//");
simulationTime.Clear();
obstacleTime.Clear();
return(true);
}
