public SmoothingResult PlanPath(LineList basePath, LineList targetPath,
List <Boundary> leftBounds, List <Boundary> rightBounds, double initHeading,
double maxSpeed, double startSpeed, double?endingHeading, CarTimestamp time, bool endingOffsetFixed)
{
PlanningSettings settings = new PlanningSettings();
settings.basePath = basePath;
settings.targetPaths = new List <Boundary>();
settings.targetPaths.Add(new Boundary(targetPath, 0, 0, settings.Options.alpha_w));
settings.leftBounds = leftBounds;
settings.rightBounds = rightBounds;
settings.initHeading = initHeading;
settings.maxSpeed = maxSpeed;
settings.startSpeed = startSpeed;
settings.endingHeading = endingHeading;
settings.timestamp = time;
settings.endingPositionFixed = endingOffsetFixed;
return(PlanPath(settings));
}
public SmoothingResult PlanPath(LineList basePath, LineList targetPath,
List<Boundary> leftBounds, List<Boundary> rightBounds, double initHeading,
double maxSpeed, double startSpeed, double? endingHeading, CarTimestamp time, bool endingOffsetFixed)
{
PlanningSettings settings = new PlanningSettings();
settings.basePath = basePath;
settings.targetPaths = new List<Boundary>();
settings.targetPaths.Add(new Boundary(targetPath, 0, 0, settings.Options.alpha_w));
settings.leftBounds = leftBounds;
settings.rightBounds = rightBounds;
settings.initHeading = initHeading;
settings.maxSpeed = maxSpeed;
settings.startSpeed = startSpeed;
settings.endingHeading = endingHeading;
settings.timestamp = time;
settings.endingPositionFixed = endingOffsetFixed;
return PlanPath(settings);
}
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);
}*/
}
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);
* }*/
}
protected void InitializePlanningSettings()
{
settings = new PlanningSettings();
smootherBasePath = null;
maxSmootherBasePathAdvancePoint = LinePath.PointOnPath.Invalid;
avoidanceBasePath = null;
maxAvoidanceBasePathAdvancePoint = LinePath.PointOnPath.Invalid;
smootherTargetPaths = new List<Boundary>();
leftLaneBounds = new List<Boundary>();
rightLaneBounds = new List<Boundary>();
additionalLeftBounds = new List<Boundary>();
additionalRightBounds = new List<Boundary>();
extraObstacles = new List<Obstacle>();
disablePathAngleCheck = false;
pathAngleCheckMax = 20;
pathAngleMax = 15 * Math.PI / 180.0;
useAvoidancePath = false;
laneWidthAtPathEnd = 3.5;
settings.Options.alpha_w = 0.00025;
settings.Options.alpha_d = 10;
settings.Options.alpha_c = 10;
}