/// <summary>
/// Predicts forward by the delay time of the tahoe
/// </summary>
/// <param name="absPose"></param>
/// <param name="vehicleState"></param>
public void ForwardPredict(double planningTime, out AbsolutePose absPose, out OperationalVehicleState vehicleState)
{
// get the current state
OperationalVehicleState currentState = Services.StateProvider.GetVehicleState();
// assume the vehicle will hold it current curvature
double curvature = TahoeParams.CalculateCurvature(currentState.steeringAngle, currentState.speed);
// simple dynamics
// xdot = v*cos(theta)
// ydot = v*sin(theta)
// thetadot = v*c
// do euler integration
double dt = 0.01;
double T = TahoeParams.actuation_delay + planningTime;
int n = (int)Math.Round(T / dt);
double v = currentState.speed;
double x = 0, y = 0, heading = 0;
for (int i = 0; i < n; i++)
{
x += v * Math.Cos(heading) * dt;
y += v * Math.Sin(heading) * dt;
heading += v * curvature * dt;
}
absPose = new AbsolutePose(new Coordinates(x, y), heading, 0);
vehicleState = currentState;
}
public SmoothingResult PlanPath(PlanningSettings settings)
{
SmootherOptions opts = settings.Options;
// for now, just run the smoothing
opts.init_heading = settings.initHeading;
opts.set_init_heading = true;
opts.min_init_velocity = settings.startSpeed * 0.5;
opts.set_min_init_velocity = true;
opts.max_init_velocity = Math.Max(settings.maxSpeed, settings.startSpeed);
opts.set_max_init_velocity = true;
opts.min_velocity = 0.1;
opts.max_velocity = Math.Max(opts.min_velocity + 0.1, settings.maxSpeed);
opts.k_max = Math.Min(TahoeParams.CalculateCurvature(-TahoeParams.SW_max, settings.startSpeed), TahoeParams.CalculateCurvature(TahoeParams.SW_max, settings.startSpeed)) * 0.97;
opts.generate_details = true; // GenerateDetails;
if (settings.endingHeading != null)
{
opts.set_final_heading = true;
opts.final_heading = settings.endingHeading.Value;
}
else
{
opts.set_final_heading = false;
}
opts.set_final_offset = settings.endingPositionFixed;
opts.final_offset_min = settings.endingPositionMin;
opts.final_offset_max = settings.endingPositionMax;
if (settings.maxEndingSpeed != null)
{
opts.set_final_velocity_max = true;
opts.final_velocity_max = Math.Max(opts.min_velocity + 0.1, settings.maxEndingSpeed.Value);
}
else
{
opts.set_final_velocity_max = false;
}
opts.a_lat_max = 6;
// create the boundary list
List <UrbanChallenge.PathSmoothing.PathPoint> ret = new List <UrbanChallenge.PathSmoothing.PathPoint>();
smoother = new PathSmoother();
OperationalTrace.WriteVerbose("calling smooth path");
SmoothResult result = smoother.SmoothPath(settings.basePath, settings.targetPaths, settings.leftBounds, settings.rightBounds, opts, ret);
if (result != SmoothResult.Sucess)
{
OperationalTrace.WriteWarning("smooth path result: {0}", result);
}
else
{
OperationalTrace.WriteVerbose("smooth path result: {0}", result);
}
AvoidanceDetails details = null;
if (opts.generate_details)
{
details = new AvoidanceDetails();
details.leftBounds = settings.leftBounds;
details.rightBounds = settings.rightBounds;
details.smoothingDetails = smoother.GetSmoothingDetails();
LastAvoidanceDetails = details;
// push out the points
Coordinates[] leftPoints = new Coordinates[details.smoothingDetails.leftBounds.Length];
for (int i = 0; i < leftPoints.Length; i++)
{
leftPoints[i] = details.smoothingDetails.leftBounds[i].point;
}
Coordinates[] rightPoints = new Coordinates[details.smoothingDetails.rightBounds.Length];
for (int i = 0; i < rightPoints.Length; i++)
{
rightPoints[i] = details.smoothingDetails.rightBounds[i].point;
}
Services.UIService.PushPoints(leftPoints, settings.timestamp, "left bound points", true);
Services.UIService.PushPoints(rightPoints, settings.timestamp, "right bound points", true);
}
//if (result == SmoothResult.Sucess) {
Coordinates[] points = new Coordinates[ret.Count];
double[] speeds = new double[ret.Count];
for (int i = 0; i < ret.Count; i++)
{
points[i] = new Coordinates(ret[i].x, ret[i].y);
speeds[i] = ret[i].v;
}
SmoothedPath path = new SmoothedPath(settings.timestamp, points, speeds);
return(new SmoothingResult(result, path, details));
/*}
* else {
* SmoothedPath path = new SmoothedPath(settings.timestamp, settings.basePath, null);
*
* return new SmoothingResult(result, path);
* }*/
}