public override void OnMouseOver( Vector2 mouseWorldPos )
{
_pointUnderMouse = -1 ;
IMainForm mainForm = summonMainForm( ) ;
for( int i = 0; i < _properties.WorldPoints.Count; i++ )
{
if( mouseWorldPos.DistanceTo( _properties.WorldPoints[ i ] ) <= 5 )
{
_pointUnderMouse = i ;
mainForm.SetCursorForCanvas( Cursors.Hand ) ;
mainForm.SetToolStripStatusLabel1(
"{0} (Point {1}: {2})".FormatWith(_properties.Name, i.ToString( ), _properties.WorldPoints[ i ].ToString( )) ) ;
}
}
if( _pointUnderMouse == -1 )
{
mainForm.SetCursorForCanvas( Cursors.Default ) ;
}
base.OnMouseOver( mouseWorldPos ) ;
}
public override void onMouseOver(Vector2 mouseworldpos)
{
pointundermouse = -1;
for (int i = 0; i < WorldPoints.Length; i++)
{
if (mouseworldpos.DistanceTo(WorldPoints[i]) <= 5)
{
pointundermouse = i;
MainForm.Instance.pictureBox1.Cursor = Cursors.Hand;
MainForm.Instance.toolStripStatusLabel1.Text = Name + " (Point " + i.ToString() + ": " + WorldPoints[i].ToString() + ")";
}
}
if (pointundermouse == -1) MainForm.Instance.pictureBox1.Cursor = Cursors.Default;
base.onMouseOver(mouseworldpos);
}
/// <summary>
/// Update rover position and remove nodes from waypoints and planned path if the rover approaches them
/// </summary>
/// <param name="pose"></param>
public void UpdatePose(RobotPose pose)
{
lock (plannerLock)
{
List<SimpleTreeNode<Vector2>> nodesToRemove = new List<SimpleTreeNode<Vector2>>();
currentLocation = pose.ToVector2();
//Remove waypoints from the list as the rover nears them
nodesToRemove.Clear();
foreach (SimpleTreeNode<Vector2> node in userWaypoints)
{
if (currentLocation.DistanceTo(node.Value) < wayPointRadius)
{
nodesToRemove.Add(node);
}
else break;
}
foreach (SimpleTreeNode<Vector2> node in nodesToRemove)
{
userWaypoints.Remove(node);
}
//Remove nodes from the planned path as the rover approaches them
lock (pathLock)
{
nodesToRemove.Clear();
foreach (SimpleTreeNode<Vector2> node in outputNodeList)
{
if (currentLocation.DistanceTo(node.Value) < pathPointRadius)
{
nodesToRemove.Add(node);
}
else break;
}
foreach (SimpleTreeNode<Vector2> node in nodesToRemove)
{
outputNodeList.Remove(node);
}
}
}
}
private static Vector2 FindNeighborToCompare(List<ILidar2DPoint> filteredScan, SensorPose lidarPose, int initialIdx, double threshold)
{
double currentX = -filteredScan[initialIdx].RThetaPoint.R * Math.Sin(filteredScan[initialIdx].RThetaPoint.theta) - lidarPose.y;
double currentY = filteredScan[initialIdx].RThetaPoint.R * Math.Cos(filteredScan[initialIdx].RThetaPoint.theta) + lidarPose.x;
double neighborX = -filteredScan[filteredScan.Count - 1].RThetaPoint.R * Math.Sin(filteredScan[filteredScan.Count - 1].RThetaPoint.theta) - lidarPose.y;
double neighborY = filteredScan[filteredScan.Count - 1].RThetaPoint.R * Math.Cos(filteredScan[filteredScan.Count - 1].RThetaPoint.theta) + lidarPose.x;
Vector2 currentPt = new Vector2(currentX, currentY);
for (int k = initialIdx; k < filteredScan.Count; k++)
{
neighborX = -filteredScan[k].RThetaPoint.R * Math.Sin(filteredScan[k].RThetaPoint.theta) - lidarPose.y;
neighborY = filteredScan[k].RThetaPoint.R * Math.Cos(filteredScan[k].RThetaPoint.theta) + lidarPose.x;
Vector2 neighborPt = new Vector2(neighborX, neighborY);
if (currentPt.DistanceTo(neighborPt) > threshold)
return neighborPt;
}
return new Vector2(neighborX, neighborY);
}
public double DistanceOffPath(Vector2 pt)
{
return Math.Abs(pt.DistanceTo(center) - radius);
}
/// <summary>
/// Gets a point on another subField that is closest to input location.
/// Distance must be less than range to be considered. Snaps to corner by 1.5
/// times more than
/// </summary>
/// <param name="location">Closest point from this location</param>
/// <param name="range">Maximum distance looking for</param>
/// <returns>CLosest point</returns>
private Vector2 getNearest(Vector2 location, double range, List<Polygon> subFields)
{
Vector2 minVP = location;
Vector2 minVL = location;
double minDP = Double.MaxValue;
double minDL = Double.MaxValue;
double dis;
foreach (Polygon p in subFields)
{
foreach (Vector2 v in p)
{
dis = location.DistanceTo(v);
if (dis < range && dis < minDP)
{
minDP = dis;
minVP = v;
}
}
DEASL.Core.Mathematics.Shapes.LineSegment l =
p.ShortestLineToOther(new Polygon(new Vector2[] { location }));
dis = l.Length;
if (dis < range && dis < minDL)
{
minDL = dis;
minVL = l.P0;
}
}
return (minDP < minDL * 1.5)? minVP : minVL;
}
public List<Waypoint> FindPath(Waypoint start, Waypoint goal, OccupancyGrid2D og, out bool success)
{
List<Waypoint> path = new List<Waypoint>();
//added by aaron (sort of a hack)
if (og == null || goal.Coordinate.DistanceTo(start.Coordinate) == 0)
{
path.Add(new Waypoint(start.Coordinate, true, 0));
success = true;
return path;
}
int xIdx, yIdx;
success = true;
Vector2[] NESWVector = new Vector2[4];
Vector2[] diagVector = new Vector2[4];
bool[] NESW = new bool[4];
Vector2 startV = start.Coordinate; // Start Vector2
Vector2 goalV = goal.Coordinate; // Goal Vector2
PriorityQueue open = new PriorityQueue();
closed = new OccupancyGrid2D(resX, resY, extentX, extentY);
opened = new OccupancyGrid2D(resX, resY, extentX, extentY);
GetIndicies(startV.X, startV.Y, out xIdx, out yIdx);
startV = new Vector2(xIdx, yIdx);
GetIndicies(goalV.X, goalV.Y, out xIdx, out yIdx);
goalV = new Vector2(xIdx, yIdx);
Node root = new Node(goalV, goalV.DistanceTo(startV), 0, null);
Node current = root;
open.Push(current);
// Do the spreading/discovering stuff until we discover a path.
while (current.xy != startV)
{
if (open.q.Count == 0 || open.q.Count > MAX_OPEN)
{
Console.WriteLine("Failure in DSstar. Open count is: " + open.q.Count);
success = false;
break;
}
current = open.Pop();
NESWVector[0] = new Vector2(current.xy.X, current.xy.Y - 1);
NESWVector[1] = new Vector2(current.xy.X + 1, current.xy.Y);
NESWVector[2] = new Vector2(current.xy.X, current.xy.Y + 1);
NESWVector[3] = new Vector2(current.xy.X - 1, current.xy.Y);
diagVector[0] = new Vector2(current.xy.X + 1, current.xy.Y - 1);
diagVector[1] = new Vector2(current.xy.X + 1, current.xy.Y + 1);
diagVector[2] = new Vector2(current.xy.X - 1, current.xy.Y + 1);
diagVector[3] = new Vector2(current.xy.X - 1, current.xy.Y - 1);
for (int i = 0; i < 4; i++)
{
if ((int)og.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) < 255)
{
if (closed.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) == 0)
{
NESW[i] = true;
if (opened.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) == 0)
{
open.Push(new Node(NESWVector[i], NESWVector[i].DistanceTo(startV), current.h + 1
+ og.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) / blurWeight, current));
opened.SetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y, 1);
}
}
}
}
for (int i = 0; i < 4; i++)
{
if (NESW[i % 4] && NESW[(i + 1) % 4])
{
if (og.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) < 255)
{
if (closed.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) == 0)
{
if (opened.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) == 0)
{
open.Push(new Node(diagVector[i], diagVector[i].DistanceTo(startV), current.h + 1.4
+ og.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) / blurWeight, current));
opened.SetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y, 1);
}
}
}
}
}
for (int i = 0; i < 4; i++) NESW[i] = false;
closed.SetCellByIdx((int) current.xy.X, (int) current.xy.Y, 1);
}
// Build a path using the discovered path.
double x, y;
Waypoint waypoint;
// First waypoint is a user waypoint
GetReals((int)current.xy.X, (int)current.xy.Y, out x, out y);
waypoint = new Waypoint(x + resX / 2, y + resY / 2, true, og.GetCellByIdx((int)current.xy.X, (int)current.xy.Y));
path.Add(waypoint);
current = current.dad;
// Middle waypoints are path waypoints
while (current != root && current != null)
{
GetReals((int) current.xy.X, (int) current.xy.Y, out x, out y);
waypoint = new Waypoint(x + resX / 2, y + resY / 2, false, og.GetCellByIdx((int)current.xy.X, (int)current.xy.Y));
path.Add(waypoint);
current = current.dad;
}
// Last waypoint is a user waypoint
if (current != null)
{
GetReals((int)current.xy.X, (int)current.xy.Y, out x, out y);
waypoint = new Waypoint(x + resX / 2, y + resY / 2, true, og.GetCellByIdx((int)current.xy.X, (int)current.xy.Y));
path.Add(waypoint);
}
return path;
}
public static void DrawEllipse(GLPen p, Vector2 firstPoint, Vector2 secondPoint, Vector2 thirdPoint)
{
float height = (float)firstPoint.DistanceTo(thirdPoint);
DrawEllipse(p, firstPoint, secondPoint, height);
//Vector2 delta = new Vector2(secondPDFPoint.X - firstPDFPoint.X, secondPDFPoint.Y - firstPDFPoint.Y);
//double thirdPointAngle = delta.ToRadians();
//thirdPDFPoint = new Vector2(mu.X + Math.Sin(thirdPointAngle) * height2, mu.Y - Math.Cos(thirdPointAngle) * height2);
}
public static void DrawEllipse(GLPen p, Vector2 firstPoint, Vector2 secondPoint, float height)
{
Vector2 deltaPoint = new Vector2(firstPoint.X - secondPoint.X, firstPoint.Y - secondPoint.Y);
double angle = deltaPoint.ToRadians();
deltaPoint = deltaPoint.Rotate90();
//Vector2 thirdPoint = deltaPoint.Normalize() * height;
float length = (float)firstPoint.DistanceTo(secondPoint);
Vector2 PreRotDrawpoint = new Vector2(0, 0);
Vector2 PostRotDrawPoint = new Vector2(0, 0);
p.GLApplyPen();
Gl.glPushMatrix();
Gl.glTranslatef((float)firstPoint.X, (float)firstPoint.Y, 0);
Gl.glBegin(Gl.GL_LINE_LOOP);
for (double i = 0; i < Math.PI * 2; i += 0.05)
{
PreRotDrawpoint = new Vector2((float)(Math.Cos(i) * length), (float)(Math.Sin(i) * height));
PostRotDrawPoint = PreRotDrawpoint.Rotate(angle);
Gl.glVertex2f((float)PostRotDrawPoint.X, (float)PostRotDrawPoint.Y);
}
Gl.glEnd();
Gl.glPopMatrix();
}
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;
}
