/// <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);
}
}
}
}
public void UpdatePose(RobotPose pose)
{
this.pose = pose;
if (waypoints == null || waypoints.Count == 0)
{
return;
}
if (waypointsAchieved >= waypoints.Count)
{
return;
}
if (pose.ToVector2().DistanceTo(waypoints.ElementAt(waypointsAchieved).Coordinate) < achieveThreshold)
{
waypoints.ElementAt(waypointsAchieved).Achieved = true;
WaypointAchieved(this, new WaypointAchievedEventArgs(waypoints[waypointsAchieved]));
waypointsAchieved++;
}
}
private void PlanPurePursuit()
{
if (pathCurrentlyTracked == null)
{
return;
}
//we are really far off the path, just get on the stupid path
//mark sucks and wants this behavior
if (pathCurrentlyTracked.Length == 0)
{
goalPoint = pathCurrentlyTracked.EndPoint.pt;
}
else if (segmentCurrentlyTracked.DistanceOffPath(currentPoint.ToVector2()) > lookAheadDistParam / 2)
{
//Console.WriteLine("2");
double lookaheadRef = 0;
PointOnPath pTemp = segmentCurrentlyTracked.StartPoint;
goalPoint = pathCurrentlyTracked.AdvancePoint(pTemp, ref lookaheadRef).pt;
}
else
{
//Console.WriteLine("1");
//see if we can track the next segment and if so, update that...
PointOnPath currentPointOnPath = segmentCurrentlyTracked.ClosestPoint(currentPoint.ToVector2());
double lookaheadRef = lookAheadDistParam;
PointOnPath lookaheadPointOnPath = pathCurrentlyTracked.AdvancePoint(currentPointOnPath, ref lookaheadRef);
//the path lookahead point is at the end, and we cant do antyhing
segmentCurrentlyTracked = lookaheadPointOnPath.segment;
goalPoint = lookaheadPointOnPath.pt;
}
double errorX = (goalPoint.X - currentPoint.x);
double errorY = (goalPoint.Y - currentPoint.y);
Vector2 verr = new Vector2(errorX, errorY);
//Console.WriteLine(errorX + " | " + errorY);
double errorDistance = verr.Length;
double currentYaw = currentPoint.yaw;
double errorYaw = UnwrapAndCalculateYawError(errorX, errorY, ref currentYaw);
//Console.Write("neg Hyst: " + hysterisisNegative + " pos Hyst: " + hysterisisPositive + " yawError" + errorYaw * 180.0/ Math.PI+ " ||");
//the reason for this is that our angles are defined from 0 to 360
//but the PID controller expects angle to be -180 to 180
//calculate the control outputs
//the idea here is we want to make the velocity (which is a derivative) be proportional to the error in our actual
//position
double unsignedYawErrorNormalizeToOne = Math.Abs(errorYaw) / Math.PI;
double commandedVelocity = (kPPosition * errorDistance);
double commandedHeading = kPHeading * errorYaw;
if (Math.Abs(commandedVelocity) > capVelocity)
{
commandedVelocity = Math.Sign(commandedVelocity) * capVelocity;
}
if (Math.Abs(commandedHeading) > capHeadingDot)
{
commandedHeading = Math.Sign(commandedHeading) * capHeadingDot;
}
//if (unsignedYawErrorNormalizeToOne > .1)
// find which input to use
RobotTwoWheelCommand currentInput = inputList[pathCurrentlyTracked.IndexOf(segmentCurrentlyTracked)];
//commandedVelocity *= (1 - Math.Pow(unsignedYawErrorNormalizeToOne, velocityTurningDamingFactor));
//command = new RobotTwoWheelCommand(commandedVelocity, commandedHeading);
if (pathCurrentlyTracked.EndPoint.pt.DistanceTo(currentPoint.ToVector2()) < .2)
{
command = new RobotTwoWheelCommand(0, 0);
}
else
{
if (currentInput.velocity < 0.3)
{
command = new RobotTwoWheelCommand(0.4, commandedHeading);
}
else
{
command = new RobotTwoWheelCommand(currentInput.velocity, commandedHeading);
}
}
//if (reachedGoal)
//{
// command.velocity = 0;
// command.turn = 0;
//}
//Console.WriteLine("Current: " + currentPoint.x + " " + currentPoint.y + " " + currentPoint.yaw + " | " + errorDistance + " | " + errorYaw + " | " + commandedVelocity + " " + commandedHeading);
//Console.WriteLine();
}
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);
}
private void GenrefVWHuniformdt(IPath path, double deltadist, double dt)
{
PointOnPath currentPointOnPath = path[0].ClosestPoint(currentPoint.ToVector2());
int numpoints = (int)((path.Length) / deltadist);
int n = 5; //number of additional points
xr = new double[numpoints + n];
yr = new double[numpoints + n];
double starttimestamp = (DateTime.Now.Ticks - 621355968000000000) / 10000000;
timestamps = new double[numpoints];
for (int i = 0; i < timestamps.Length; i++)
{
timestamps[i] = starttimestamp + i * dt;
}
for (int i = 0; i <= numpoints + 3; i++)
{
double d = deltadist * (i);
PointOnPath lookaheadpointi = path.AdvancePoint(currentPointOnPath, ref d);
xr[i] = lookaheadpointi.pt.X;
yr[i] = lookaheadpointi.pt.Y;
}
double[] xrdot = new double[xr.Length - 1];
double[] yrdot = new double[xrdot.Length];
double[] xr2dot = new double[xrdot.Length - 1];
double[] yr2dot = new double[xr2dot.Length];
double[] vr = new double[xr.Length];
double[] wr = new double[xr.Length];
double[] hr = new double[xr.Length];
for (int i = 0; i < xr.Length - 1; i++)
{
xrdot[i] = (xr[i + 1] - xr[i]) / dt;
yrdot[i] = (yr[i + 1] - yr[i]) / dt;
}
for (int i = 0; i < xrdot.Length - 1; i++)
{
xr2dot[i] = (xrdot[i + 1] - xrdot[i]) / dt;
yr2dot[i] = (yrdot[i + 1] - yrdot[i]) / dt;
}
for (int i = 0; i < xrdot.Length; i++)
{
vr[i] = Math.Sqrt(Math.Pow(xrdot[i], 2) + Math.Pow(yrdot[i], 2));
hr[i] = Math.Atan2(yrdot[i], xrdot[i]); // in radians
// unwrap headings
if (i > 0)
{
while (hr[i] - hr[i - 1] >= Math.PI)
{
hr[i] -= 2 * Math.PI;;
}
while (hr[i] - hr[i - 1] <= -Math.PI)
{
hr[i] += 2 * Math.PI;
}
}
if (i < xr2dot.Length)
{
wr[i] = ((xrdot[i] * yr2dot[i] - yrdot[i] * xr2dot[i]) / (Math.Pow(vr[i], 2))); // in radians/sec
}
}
//pad the reference vectors
for (int i = 1; i < n; i++)
{
wr[numpoints + i] = 0;
vr[numpoints + i] = 0;
hr[numpoints + i] = 0;
}
TextWriter tw = new StreamWriter("parametrization.txt");
for (int i = 0; i < timestamps.Length; i++)
{
tw.WriteLine("index timestamp x y vr wr hr");
tw.WriteLine("{0} {1} {2} {3} {4} {5} {6}", i, timestamps[i], xr[i], yr[i], vr[i], wr[i], hr[i]);
}
tw.Close();
}
public RobotTwoWheelCommand GetCommand(double transMax, double turnMax)
{
lock (followerLock)
{
if (pathCurrentlyTracked == null)
{
Console.WriteLine("Null path tracked!");
return(new RobotTwoWheelCommand(0, 0));
}
//we are really far off the path, just get on the stupid path
//mark sucks and wants this behavior
if (pathCurrentlyTracked.Length == 0) //single waypoint case
{
goalPoint = pathCurrentlyTracked.EndPoint.pt;
startPoint = pathCurrentlyTracked.EndPoint.pt;
}
//else if (segmentCurrentlyTracked.DistanceOffPath(currentPoint.ToVector2()) > lookAheadDistParam / 2)
//{
// //Console.WriteLine("2");
// double lookaheadRef = 0;
// PointOnPath pTemp = segmentCurrentlyTracked.StartPoint;
// goalPoint = pathCurrentlyTracked.AdvancePoint(pTemp, ref lookaheadRef).pt;
//}
else
{
//Console.WriteLine("1");
//see if we can track the next segment and if so, update that...
PointOnPath currentPointOnPath = segmentCurrentlyTracked.ClosestPoint(currentPoint.ToVector2());
double lookaheadRef = lookAheadDistParam;
PointOnPath lookaheadPointOnPath = pathCurrentlyTracked.AdvancePoint(currentPointOnPath, ref lookaheadRef);
if (segmentCurrentlyTracked != lookaheadPointOnPath.segment)
{
segmentCurrentlyTracked = lookaheadPointOnPath.segment;
}
goalPoint = lookaheadPointOnPath.pt;
startPoint = currentPointOnPath.pt;
}
// Calculate errors
double errorX = (goalPoint.X - currentPoint.x);
double errorY = (goalPoint.Y - currentPoint.y);
Vector2 verr = new Vector2(errorX, errorY);
double tangentX = (goalPoint.X - startPoint.X);
double tangentY = (goalPoint.Y - startPoint.Y);
Vector2 tangent = new Vector2(tangentX, tangentY);
double errorDistance = currentPoint.ToVector2().DistanceTo(startPoint);
double currentYaw = currentPoint.yaw;
double tempCurrentYaw = currentPoint.yaw;
errorYawTangent = UnwrapAndCalculateYawError(tangentX, tangentY, ref tempCurrentYaw);
errorYaw = UnwrapAndCalculateYawError(errorX, errorY, ref currentYaw);
double tangentEquation = (goalPoint.Y - startPoint.Y) * currentPoint.x / (goalPoint.X - startPoint.Y)
- (goalPoint.Y - startPoint.Y) * goalPoint.X / (goalPoint.X - startPoint.Y) + startPoint.Y;
//Console.Clear();
//Console.WriteLine("Current yaw is: " + currentYaw);
if (tangentEquation < currentPoint.y)
{
if (Math.PI / 2 <= Math.Abs(currentYaw) && Math.Abs(currentYaw) <= Math.PI)
{
//Console.WriteLine("Above line, pointing left");
errorDistance *= -1;
}
//else Console.WriteLine("Above line, pointing right");
}
else
{
if (0 <= Math.Abs(currentYaw) && Math.Abs(currentYaw) <= Math.PI / 2)
{
//Console.WriteLine("Below line, pointing right");
errorDistance *= -1;
}
//else Console.Write("Below line, pointing left");
}
//if we are really close to last waypoint, make the robot just stop
if ((segmentCurrentlyTracked == pathCurrentlyTracked[pathCurrentlyTracked.Count - 1]) &&
(PathCurrentlyTracked.EndPoint.pt - currentPoint.ToVector2()).Length < lookAheadDistParam)
{
command = new RobotTwoWheelCommand(0, 0);
//Console.WriteLine("Acheived!");
}
else
{
//the idea here is we want to make the velocity (which is a derivative) be proportional to the error in our actual
//position
double unsignedYawErrorNormalizeToOne = Math.Abs(errorYaw) / Math.PI;
double velocityPercentage = Math.Abs((Math.PI - Math.Abs(errorYawTangent)) / Math.PI);
double commandedVelocity = (velocityPercentage < 0.5) ? 0.0 : transMax * velocityPercentage;
//double commandedHeading = 200 * errorYawTangent + 100 * errorYaw + 300 * errorDistance;
double commandedHeading = 150 * errorYawTangent + 100 * errorYaw + 200 * errorDistance;
if (Math.Abs(commandedVelocity) > transMax)
{
commandedVelocity = Math.Sign(commandedVelocity) * transMax;
}
if (Math.Abs(commandedHeading) > turnMax)
{
commandedHeading = Math.Sign(commandedHeading) * turnMax;
commandedVelocity = 0.0;
}
/*if (unsignedYawErrorNormalizeToOne > .1)
* commandedVelocity *= (1 - Math.Pow(unsignedYawErrorNormalizeToOne, velocityTurningDamingFactor));*/
command = new RobotTwoWheelCommand(commandedVelocity, commandedHeading);
//Console.WriteLine(errorYaw + ", " + errorYawTangent + ", " + errorDistance);
}
return(command);
}
}
