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(SmoothResult result, SmoothedPath path, AvoidanceDetails details)
{
this.result = result;
this.path = path;
this.details = details;
}
private void menuGetDetails_Click(object sender, EventArgs e)
{
try {
avoidanceDetails = OperationalInterface.OperationalUIFacade.DebuggingFacade.GetAvoidanceDetails();
if (avoidanceDetails == null) {
MessageBox.Show("Operational is not generating avoidance details");
}
}
catch (Exception ex) {
MessageBox.Show("Error getting avoidance details:\n" + ex.Message);
}
}
public void Clear()
{
avoidanceDetails = null;
}
protected override PlanningResult OnDynamicallyInfeasible(IList<Obstacle> obstacles, AvoidanceDetails details)
{
if (sparse && smootherSpacingAdjust > -1) {
smootherSpacingAdjust = Math.Max(-1, smootherSpacingAdjust - 0.05);
return base.OnDynamicallyInfeasible(obstacles, details, false);
}
else {
return base.OnDynamicallyInfeasible(obstacles, details);
}
}
protected virtual PlanningResult OnDynamicallyInfeasible(IList<Obstacle> obstacles, AvoidanceDetails details, bool sendBlockage)
{
double scalarSpeed = -1;
if (speedCommand is ScalarSpeedCommand) {
scalarSpeed = ((ScalarSpeedCommand)speedCommand).Speed;
}
// see if we can figure out a stop distance based on stuff crossing
double stopDist = double.NaN;
try {
if (details != null) {
double totalDist = 0;
int numBounds = details.smoothingDetails.leftBounds.Length;
for (int i = 0; i < numBounds; i++) {
// left is less than right, we can't make it through
if (details.smoothingDetails.leftBounds[i].deviation < details.smoothingDetails.rightBounds[i].deviation) {
stopDist = totalDist;
break;
}
totalDist += 0.5;
}
}
}
catch (Exception) {
}
if (vs.IsStopped && scalarSpeed != 0 && sendBlockage && !Services.BehaviorManager.TestMode) {
if (lastDynInfeasibleTime == null) {
lastDynInfeasibleTime = HighResDateTime.Now;
}
else if (HighResDateTime.Now - lastDynInfeasibleTime.Value > TimeSpan.FromSeconds(2)) {
// send a completion report with the error
bool stopTooClose = stopDist < TahoeParams.VL || double.IsNaN(stopDist);
CompletionReport report = new TrajectoryBlockedReport(UrbanChallenge.Behaviors.CompletionReport.CompletionResult.Stopped, stopDist, BlockageType.Unknown, -1, stopTooClose || DetermineReverseRecommended(obstacles), Services.BehaviorManager.CurrentBehaviorType);
ForwardCompletionReport(report);
}
}
else {
lastDynInfeasibleTime = null;
}
if (Services.BehaviorManager.TestMode) {
bool stopTooClose = stopDist < TahoeParams.VL || double.IsNaN(stopDist);
CompletionReport report = new TrajectoryBlockedReport(UrbanChallenge.Behaviors.CompletionReport.CompletionResult.Stopped, stopDist, BlockageType.Unknown, -1, stopTooClose || DetermineReverseRecommended(obstacles), Services.BehaviorManager.CurrentBehaviorType);
ForwardCompletionReport(report);
}
stopDist -= 2;
if (stopDist < 0.01) {
stopDist = 0.01;
}
double nomDecel = 3;
if (scalarSpeed == 0) {
nomDecel = 4;
}
// nominal stopping acceleration is 3 m/s^2
double nomStoppingDist = vs.speed*vs.speed/(2*nomDecel);
if (double.IsNaN(stopDist) || stopDist > nomStoppingDist) {
stopDist = nomStoppingDist;
}
// figure out the target deceleration
double targetDecel = vs.speed*vs.speed/(2*stopDist);
PlanningResult result = new PlanningResult();
// figure out if arbiter is already stopping shorter
double commandedStopDist = GetSpeedCommandStopDistance();
if (!double.IsPositiveInfinity(commandedStopDist) && commandedStopDist < stopDist) {
// don't need to do anything, we're already stopping appropriately
result.speedCommandGenerator = null;
}
else if (vs.IsStopped) {
// just hold the brakes with the standard pressure
result.speedCommandGenerator = new ConstantSpeedCommandGenerator(0, TahoeParams.brake_hold+1);
}
else {
// do a constant deceleration profile
result.speedCommandGenerator = new FeedbackSpeedCommandGenerator(new ConstantAccelSpeedGenerator(-targetDecel));
}
if (prevSmoothedPath != null) {
RelativeTransform transform = Services.RelativePose.GetTransform(prevSmoothedPathTimestamp, curTimestamp);
SmoothedPath smoothedPath = new SmoothedPath(curTimestamp, prevSmoothedPath.Transform(transform), null);
result.steeringCommandGenerator = new PathSteeringCommandGenerator(smoothedPath);
result.smoothedPath = smoothedPath;
}
else {
result.steeringCommandGenerator = new ConstantSteeringCommandGenerator(null, null, false);
}
result.dynamicallyInfeasible = true;
result.commandLabel = "dynamically infeasible";
return result;
}
protected virtual PlanningResult OnDynamicallyInfeasible(IList<Obstacle> obstacles, AvoidanceDetails details)
{
return OnDynamicallyInfeasible(obstacles, details, true);
}
