public RRTNode GetNodeToGoal()
{
if (waypoints.Length == 0.0)
return null;
lock (locker)
{
RobotTwoWheelState currentState = stateProvider.GetCurrentState(lastCommand);
IPath path = new PointPath();
RRTNode startingNode = new RRTNode(currentState);
RRTNode goalNode;
//foreach (IPathSegment segment in waypoints)
//{
bool foundPath = false;
Console.WriteLine("Finding a path");
do
{
startingNode = new RRTNode(currentState);
foundPath = rrt.FindPath(ref startingNode, waypoints[1].End, obstacles, out goalNode);
}
while (!foundPath && receivedNewPath);
receivedNewPath = false;
return goalNode;
}
}
public PointPath ToPointPath()
{
PointPath p = new PointPath();
foreach (LineSegment seg in GetSegmentEnumerator())
{
p.Add(new LinePathSegment(seg.P0, seg.P1));
}
return(p);
}
/// <summary>
/// walks along the bezier path and creates a straight line segment approximation
/// </summary>
/// <param name="path"></param>
/// <param name="sampleDistance"></param>
/// <returns></returns>
public PointPath ConvertBezierPathToPointPath(IPath path, double sampleDistance)
{
PointPath pathOut = new PointPath();
foreach (IPathSegment seg in path)
{
if (seg is BezierPathSegment == false) throw new InvalidOperationException();
BezierPathSegment bseg = seg as BezierPathSegment;
PointOnPath p = seg.StartPoint;
double refDist = 0;
while (refDist == 0)
{
PointOnPath p2;
refDist = sampleDistance;
p2 = seg.AdvancePoint(p, ref refDist);
pathOut.Add(new LinePathSegment(p.pt, p2.pt));
p = p2;
}
}
return pathOut;
}
public IPath GetPathToGoal()
{
if (waypoints.Length == 0.0)
return null;
lock (locker)
{
RobotTwoWheelState currentState = stateProvider.GetCurrentState(lastCommand);
IPath path = new PointPath();
RRTNode startingNode = new RRTNode(currentState);
RRTNode goalNode;
//foreach (IPathSegment segment in waypoints)
//{
while (rrt.FindPath(ref startingNode, waypoints[1].End, obstacles, out goalNode) == false)
{
continue;
}
//startingNode = goalNode;
IPath segmentToGoal = GetFinalPath(goalNode);
// add each small segment of each segment to the path to return
//foreach (IPathSegment seg in segmentToGoal)
//path.Add(seg);
//}
return segmentToGoal;
}
}
/// <summary>
/// Find the path from the goal to the starting point
/// </summary>
/// <param name="goal"></param>
/// <returns>path from the start point to the goal point</returns>
private IPath GetFinalPath(RRTNode goal)
{
IPath pathToGoal = new PointPath();
GetToRoot(goal, ref pathToGoal);
return pathToGoal;
}
/// <summary>
/// Output the last planned path
/// </summary>
public IPath GetPathToGoal()
{
lock (pathLock)
{
PointPath newPath = new PointPath();
if (outputNodeList.Count == 1)
{
//If there is only one ode in the planned path, return an IPath with a degenerate segment
newPath.Add(new LinePathSegment(outputNodeList[0].Value, outputNodeList[0].Value));
}
else if (outputNodeList.Count > 1)
{
//Build an IPath based on the values of each node in the last planned path
int segmentCount = outputNodeList.Count - 1;
for (int i = 0; i < segmentCount; i++)
{
newPath.Add(new LinePathSegment(outputNodeList[i].Value, outputNodeList[i + 1].Value));
}
}
return newPath;
}
}
public void UpdatePath(RRTNode goalNode)
{
//create IPath from rootNode
IPath newNodePath = new PointPath();
inputList = new List<RobotTwoWheelCommand>();
ConvertNodeToPath(newNodePath, goalNode, inputList);
lock (followerLock)
{
UpdatePath(newNodePath);
}
}
public IPath GetPathToGoal(IPath[] otherPaths, out IPath sparsePath, out bool inObstacle, out bool wpInObstacle)
{
inObstacle = false;
wpInObstacle = false;
sparsePath = null;
if (waypoints == null) return null;
if (waypointsAchieved >= waypoints.Count) return null;
if (og == null) return null;
if (bloatedObstacles == null)
{
Console.WriteLine("Convolved obstacles fail!");
return null;
}
// Add current pose position to filteredWaypoints. (FilteredWaypoints is what we will be
// planning over. Waypoints is list of user given waypoints.)
List<Waypoint> filteredWaypoints = new List<Waypoint>();
filteredWaypoints.Add(new Waypoint(pose.ToVector2(), true, 0));
lock (ogLock)
{
if (og.GetCell(pose.x, pose.y) == 255)
{
inObstacle = true;
og.SetCell(pose.x, pose.y, 0);
}
}
// Only add user waypoints to PathPoints if they have not been achieved yet.
// If they are inside an obstacle, return a null path.
foreach (Waypoint wp in waypoints)
{
if (!wp.Achieved)
{
lock (bloatedObstacleLock)
{
foreach (Polygon p in bloatedObstacles)
{
if (p.IsInside(wp.Coordinate))
{
wpInObstacle = true;
return null;
}
}
}
filteredWaypoints.Add(wp);
}
}
// Plan each segment of the filteredWaypoints individually and stitch them together.
List<Waypoint> completeRawPath = new List<Waypoint>();
for (int i = 0; i < filteredWaypoints.Count - 1; i++)
{
bool pathOk;
List<Waypoint> rawPath;
lock (ogLock)
{
rawPath = dstar.FindPath(filteredWaypoints[i], filteredWaypoints[i + 1], og, out pathOk);
}
if (!pathOk) return null;
else
{
int j;
// We want to only add the first path waypoint if it is the first
// segment of the whole path.
if (i == 0) j = 0;
else j = 1;
for (; j < rawPath.Count; j++)
completeRawPath.Add(rawPath.ElementAt(j));
}
}
if (completeRawPath.Count > 0)
{
IPath bezierPath;
PointPath segmentedBezierPath;
lock (bloatedObstacleLock)
{
bezierPath = smoother.Wp2BezierPath(completeRawPath, bloatedObstacles, collapseThreshold);
}
sparsePath = new PointPath(completeRawPath);
segmentedBezierPath = smoother.ConvertBezierPathToPointPath(bezierPath, bezierSegmentation);
// We want to check our sparse path against other sparse paths to see
// if there are any intersections. If there are, we want to know if the
// intersections will be a problem. If it is a problem, bloat that sparse path,
// add it to obstacles, and replan.
for (int i = 0; i < otherPaths.Length; i++)
{
if (otherPaths[i] != null && otherPaths[i].Count > 0 && i + 1 != robotID)
{
double intersectDist = DoPathsCollide(sparsePath, otherPaths[i]);
if (intersectDist != -1)
{
PointOnPath collisionPt;
int collisionSegmentIndex;
double tempDist = intersectDist - collideBackoff;
tempDist = Math.Max(0, tempDist);
collisionPt = segmentedBezierPath.AdvancePoint(segmentedBezierPath.StartPoint, ref tempDist);
collisionSegmentIndex = segmentedBezierPath.IndexOf(collisionPt.segment);
segmentedBezierPath.RemoveRange(collisionSegmentIndex, segmentedBezierPath.Count - collisionSegmentIndex);
tempDist = intersectDist - collideBackoff;
tempDist = Math.Max(0, tempDist);
collisionPt = sparsePath.AdvancePoint(sparsePath.StartPoint, ref tempDist);
collisionSegmentIndex = sparsePath.IndexOf(collisionPt.segment);
sparsePath.RemoveRange(collisionSegmentIndex, sparsePath.Count - collisionSegmentIndex - 1);
sparsePath[collisionSegmentIndex].End = collisionPt.pt;
}
}
}
return segmentedBezierPath;
}
return null;
}
public IPath Wp2BezierPath(List<Waypoint> path, List<Polygon> obstacles, double collapseThreshold)
{
List<IPathSegment> segments = new List<IPathSegment>();
List<Vector2> newPath = new List<Vector2>();
PointPath bezPath;
Vector2 P0, P1, P2, P3, dXY, newPoint;
dXY = new Vector2(0, 0);
double previousCellValue = path.ElementAt(0).TerrainCost;
bool inPlateau = false, inLocalMin = false;
double localMin = Double.MaxValue, localMax = Double.MinValue;
// Go through the path and mark local minimas and maximas
for (int i = 1; i < path.Count; i++)
{
if (inPlateau)
{
if (previousCellValue != path.ElementAt(i).TerrainCost)
{
inPlateau = false;
//path.ElementAt(i - 1).Critical = true;
}
}
else
{
if (previousCellValue == path.ElementAt(i).TerrainCost)
{
inPlateau = true;
//path.ElementAt(i - 1).Critical = true;
}
if (inLocalMin)
{
if (path.ElementAt(i).TerrainCost < localMin)
localMin = path.ElementAt(i).TerrainCost;
else
{
localMin = Double.MaxValue;
inLocalMin = false;
//path.ElementAt(i - 1).Critical = true;
}
}
else
{
if (path.ElementAt(i).TerrainCost > localMax)
localMax = path.ElementAt(i).TerrainCost;
else
{
localMax = Double.MinValue;
inLocalMin = true;
path.ElementAt(i - 1).Critical = true;
}
}
}
previousCellValue = path.ElementAt(i).TerrainCost;
}
// Collapse nodes down based on proximity to one another
for (int i = 0; i < path.Count - 1; i++)
{
if (path.ElementAt(i).Coordinate.DistanceTo(path.ElementAt(i + 1).Coordinate) < collapseThreshold)
{
if (!path.ElementAt(i).UserWaypoint && !path.ElementAt(i).Critical)
{
if (!path.ElementAt(i + 1).UserWaypoint && !path.ElementAt(i + 1).Critical)
{
newPoint = (path.ElementAt(i).Coordinate + path.ElementAt(i + 1).Coordinate) / 2;
if (IsClear(newPoint, path.ElementAt(i - 1).Coordinate, obstacles)
&& IsClear(newPoint, path.ElementAt(i + 2).Coordinate, obstacles))
{
path.RemoveRange(i, 2);
path.Insert(i, new Waypoint(newPoint, false, 0));
i--;
}
}
else
{
newPoint = path.ElementAt(i + 1).Coordinate;
if (IsClear(newPoint, path.ElementAt(i - 1).Coordinate, obstacles))
{
path.RemoveAt(i);
i--;
}
}
}
else
{
if (!path.ElementAt(i + 1).UserWaypoint && !path.ElementAt(i + 1).Critical)
{
newPoint = path.ElementAt(i).Coordinate;
if (IsClear(newPoint, path.ElementAt(i + 2).Coordinate, obstacles))
{
path.RemoveAt(i + 1);
i--;
}
}
}
}
}
// Change waypoints to Bezier curves
for (int i = path.Count - 1; i > 0; i--)
{
P3 = path.ElementAt(i).Coordinate;
P0 = path.ElementAt(i - 1).Coordinate;
if (i == path.Count - 1)
P2 = P3 - ((P3 - P0) / 3);
else
{
if (dXY.Length > ((P3 - P0) / 4).Length)
dXY *= ((P3 - P0) / 4).Length / dXY.Length;
P2 = P3 - dXY;
}
if (i == 1)
P1 = P0 + ((P3 - P0) / 3);
else
{
dXY = (P3 - path.ElementAt(i - 2).Coordinate) / 6;
if (dXY.Length > ((P3 - P0) / 4).Length)
dXY *= ((P3 - P0) / 4).Length / dXY.Length;
P1 = P0 + dXY;
}
segments.Add(new BezierPathSegment(P0, P1, P2, P3, 0.05, false));
}
segments.Reverse();
bezPath = new PointPath(segments);
return bezPath;
}
public IPath Wp2CubicPath(IPath path, List<Polygon> obstacles, double nothing)
{
PointPath ret = new PointPath();
CubicBezier[] bez = SmoothingSpline.BuildCardinalSpline(PathUtils.IPathToPoints(path).ToArray(), null, null, 1.0);
foreach (CubicBezier b in bez)
{
ret.Add(new BezierPathSegment(b, null, false));
}
return ret;
}
