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); }