/// <summary>
/// Cleans the path. Removes all not needed entries from the Path.
/// The returned path is a copy of the orignal nodes!
/// </summary>
/// <returns>The path.</returns>
/// <param name="Path">Path.</param>
public static RRTPath CleanPath(RRTPath Path)
{
List <RRTNode> nodes = Path.ToList();
//Clone all nodes
List <RRTNode> clonedNodes = new List <RRTNode>();
foreach (var item in nodes)
{
clonedNodes.Add(item.Clone());
}
//Reconnect the nodes
//First item is end
//Last item is start
RRTNode start = clonedNodes[0];
RRTNode end = clonedNodes[clonedNodes.Count - 1];
for (int i = 0; i < clonedNodes.Count - 1; i++)
{
clonedNodes [i].Predecessor = clonedNodes [i + 1];
clonedNodes[i + 1].AddSucessor(clonedNodes[i]);
}
RRTPath cleanedPath = new RRTPath(start, end);
cleanedPath.Color = Path.Color;
cleanedPath.DistanceToEnd = Path.DistanceToEnd;
cleanedPath.OrientationDeviation = Path.OrientationDeviation;
return(cleanedPath);
}
/// <summary>
/// Initializes a new instance of the <see cref="BRRT.PathOptimizer"/> class.
/// </summary>
/// <param name="_Path">Path.</param>
/// <param name="_Map">Map.</param>
/// <param name="_EndPoint">End point.</param>
public PathOptimizer(RRTPath _Path, Map _Map, RRTNode _EndPoint)
{
this.InternalMap = _Map;
this.Path = _Path;
this.Iterations = 30000;
this.MaximumDriftAngle = 10;
this.MinimumRadius = 20;
this.AllowedOrientationDeviation = 1;
this.StepWidthStraight = 15;
this.EndPoint = _EndPoint;
this.StepWidthEnd = 4;
this.StepWidthCurve = 10;
}
/// <summary>
/// Draws the given RRTPath.
/// </summary>
/// <param name="Path">Path.</param>
/// <param name="_Map">Map.</param>
/// <param name="PathPen">Path pen.</param>
public static void DrawPath(RRTPath Path, Map _Map, Pen PathPen)
{
RRTNode previous = Path.Start;
Graphics g = Graphics.FromImage(_Map.ImageMap);
while (previous != null)
{
RRTHelpers.DrawImportantNode(previous, _Map, 2, Path.Color);
if (previous.Predecessor != null)
{
g.DrawLine(PathPen, _Map.ToMapCoordinates(previous.Position), _Map.ToMapCoordinates(previous.Predecessor.Position));
}
previous = previous.Predecessor;
}
}
public static int Main(string[] args)
{
//Stopwatch for getting the total execution time
Stopwatch totalTimeWatch = new Stopwatch();
totalTimeWatch.Start();
if (args.Length > 3)
{
//Parse arguments
//1. Path 2. Start 3. Stop 4. Output path 5. path to xml document with path (optional)
string MapPath = args [0];
string start = args [1]; //x,y,phi
start = start.Trim();
string stop = args [2]; // x, y, phi
stop = stop.Trim();
string outputPath = args[3];
string[] startArray = start.Split(new char[] { ',' });
string[] stopArray = stop.Split(new char[] { ',' });
Point StartPoint = new Point(Convert.ToInt32(startArray [0]), Convert.ToInt32(startArray [1]));
double StartOrientation = Convert.ToDouble(startArray [2]);
Point StopPoint = new Point(Convert.ToInt32(stopArray [0]), Convert.ToInt32(stopArray [1]));
double StopOrientation = Convert.ToDouble(stopArray [2]);
Console.WriteLine("Start is: " + StartPoint.ToString() + " , " + StartOrientation);
Console.WriteLine("Stop is: " + StopPoint.ToString() + " , " + StopOrientation);
//Check if a path for the xml document was given.
string pathXml = "";
if (args.Length > 4)
{
pathXml = args [4];
Console.WriteLine("Saving path to: " + pathXml);
}
//In case the map exists
if (File.Exists(MapPath))
{
//Stopwatch for timing
Stopwatch watch = new Stopwatch();
watch.Start();
//Load bitmap
Bitmap BitMap = ImageHelpers.CreateNonIndexedImage(new Bitmap(MapPath));
//Create new map from bitmap
Map MyMap = new Map(BitMap);
watch.Stop();
Console.WriteLine("Loading Map took: " + watch.ElapsedMilliseconds + " ms");
watch.Reset();
//Create new instance of algorithm
RRT Algorithm = new RRT(MyMap);
//Callback (lambda function) when the rrt algorithm has finished
Algorithm.Finished += (object sender, EventArgs e) => {
//Stop timing stopwatch
watch.Stop();
Console.WriteLine("Algorithm took: " + watch.ElapsedMilliseconds + " ms");
//Event that gets called when the RRT is finished
Console.WriteLine("Finished");
watch.Reset();
watch.Start();
//Look for paths from the endpoint to the start
List <RRTPath> paths = Algorithm.FindPathToTarget();
watch.Stop();
Console.WriteLine("Finding path took: " + watch.ElapsedMilliseconds + " ms");
//In case we found a path
if (paths.Count > 0)
{
//ATTENTION: Path.Start is the point next to given EndPoint
// Path.End is the StartPoint
//Clean the path (Remove all branches in the tree we dont want to follow)
RRTPath shortestPath = RRTPath.CleanPath(paths[0]);
shortestPath.Color = Color.Red;
//Draw the path
RRTHelpers.DrawPath(shortestPath, MyMap, Pens.Cyan);
//Ok optimize with the currently best path
Console.WriteLine("Starting Optimization");
//Create new instance of optimizer
PathOptimizer optimizer = new PathOptimizer(shortestPath, MyMap, Algorithm.EndRRTNode);
watch.Reset();
watch.Start();
//Optimize the path
optimizer.Optimize();
watch.Stop();
Console.WriteLine("Optimizing took: " + watch.ElapsedMilliseconds + " ms");
optimizer.Path.Color = Color.Blue;
//Draw the path
RRTHelpers.DrawPath(optimizer.Path, MyMap, Pens.DarkGoldenrod);
//Save it into a file if a xml path was given
if (pathXml != "")
{
optimizer.Path.SaveToFile(pathXml);
}
}
else
{
Console.WriteLine("No paths found");
}
watch.Reset();
watch.Start();
//Draw the legend in the upper left corner
MyMap.DrawLegend();
//Draw the tree on the map
//RRTHelpers.DrawTree(Algorithm.StartRRTNode, MyMap);
//Draw the start point
RRTHelpers.DrawImportantNode(Algorithm.StartRRTNode, MyMap, 5, Color.Red);
//Draw the endpoint
RRTHelpers.DrawImportantNode(Algorithm.EndRRTNode, MyMap, 5, Color.Aqua);
watch.Stop();
Console.WriteLine("Drawing took: " + watch.ElapsedMilliseconds + " ms");
if (paths.Count > 0)
{
watch.Reset();
watch.Start();
//Save the result
MyMap.SaveBitmapToFile(outputPath);
watch.Stop();
Console.WriteLine("Saving result to image took: " + watch.ElapsedMilliseconds + " ms");
}
};
//Start the RRT
watch.Start();
Algorithm.Start(StartPoint, StartOrientation, StopPoint, StopOrientation);
}
}
totalTimeWatch.Stop();
Console.WriteLine("Totaltime was: " + totalTimeWatch.ElapsedMilliseconds + " ms");
return(0);
}
/// <summary>
/// Find a path from the endpoint to the startpoint.
/// This is done by looking at all points and selecting the points that are in the given target area.
/// Their orientation may only vary by the given AcceptableOrientationDeviation.
/// </summary>
/// <returns>The path to target.</returns>
public List <RRTPath> FindPathToTarget()
{
//Move area around the endpoint
Rectangle TranslatedTargetArea = new Rectangle(EndPoint.X - TargetArea.Width / 2, EndPoint.Y + TargetArea.Height / 2, TargetArea.Width, TargetArea.Height);
Graphics g = Graphics.FromImage(InternalMap.ImageMap);
g.DrawRectangle(Pens.Fuchsia, new Rectangle(InternalMap.ToMapCoordinates(TranslatedTargetArea.Location), TranslatedTargetArea.Size));
List <RRTNode> NodesInTargetArea = new List <RRTNode>();
List <RRTPath> Paths = new List <RRTPath>();
//Step through all nodes
foreach (var item in AllNodes)
{
//Check if the rectangle contains the point and check the orientation
if (Math.Abs(item.Orientation - EndRRTNode.Orientation) < AcceptableOrientationDeviation)
{
if ((Math.Abs(EndPoint.X - item.Position.X) < TargetArea.Width / 2) && (Math.Abs(EndPoint.Y - item.Position.Y) < TargetArea.Height / 2))
{
//Add to the list of found nodes.
NodesInTargetArea.Add(item);
Console.WriteLine("Found node in target area: " + item);
}
}
}
//In case no point was found return
if (NodesInTargetArea.Count == 0)
{
return(Paths);
}
//Some helpers for creating a nice color for the paths ;)
int B = 0;
int R = 255;
int Step = 255 / NodesInTargetArea.Count;
//Step through all found nodes
foreach (var item in NodesInTargetArea)
{
//Calculate length
double length = 0;
//Follow the Predecessor until their is none (this is the startpoint then)
RRTNode previous = item.Predecessor;
RRTNode end = null;
while (previous != null)
{
if (previous.Predecessor != null)
{
//TODO replace with RRTHelpers.CalculateDistance
length += RRTHelpers.CalculateDistance(previous, previous.Predecessor);
//length += Math.Sqrt (Math.Pow (previous.Position.X - previous.Predecessor.Position.X, 2) + Math.Pow (previous.Position.Y - previous.Predecessor.Position.Y, 2));
}
else
{
end = previous;
}
previous = previous.Predecessor;
}
//Create new path from start and end item
RRTPath path = new RRTPath(item, end);
Paths.Add(path);
path.Color = Color.FromArgb(R, 50, B);
path.DistanceToEnd = RRTHelpers.CalculateDistance(path.Start, EndRRTNode);
path.OrientationDeviation = path.Start.Orientation - EndRRTNode.Orientation;
B += Step;
R -= Step;
}
//Sort the list by the cost function of the given paths
Paths.Sort((RRTPath x, RRTPath y) => {
if (x.Cost() > y.Cost())
{
return(1);
}
else
{
return(-1);
}
});
//List<RRTPath> SortedList = Paths.AsParallel().OrderBy(o => o.Cost()).ToList();
foreach (var item in Paths)
{
Console.WriteLine("Length for path " + item.Color.ToString() + " : " + item.Length + " Distance to End: " + item.DistanceToEnd + " OrientationDif: " + item.OrientationDeviation);
}
return(Paths);
}