public SmoothingResult(SmoothResult result, SmoothedPath path, AvoidanceDetails details) { this.result = result; this.path = path; this.details = details; }
public static TrackingCommand GetSmoothedPathVelocityCommand(SmoothedPath path) { return new TrackingCommand(new FeedbackSpeedCommandGenerator(path), new PathSteeringCommandGenerator(path), false); }
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 PlanningResult Smooth(bool doAvoidance) { double curSpeed = vs.speed; Services.Dataset.ItemAs<double>("extra spacing").Add(Services.ObstaclePipeline.ExtraSpacing, curTimestamp); Services.Dataset.ItemAs<double>("smoother spacing adjust").Add(smootherSpacingAdjust, curTimestamp); // get the tracking manager to predict stuff like whoa AbsolutePose absPose; OperationalVehicleState vehicleState; double averageCycleTime = Services.BehaviorManager.AverageCycleTime; Services.TrackingManager.ForwardPredict(averageCycleTime, out absPose, out vehicleState); Services.Dataset.ItemAs<double>("planning cycle time").Add(averageCycleTime, LocalCarTimeProvider.LocalNow); settings.initHeading = absPose.heading; smootherBasePath = ReplaceFirstPoint(smootherBasePath, maxSmootherBasePathAdvancePoint, absPose.xy); if (avoidanceBasePath != null && avoidanceBasePath.Count > 0) { avoidanceBasePath = ReplaceFirstPoint(avoidanceBasePath, maxAvoidanceBasePathAdvancePoint, absPose.xy); } else { avoidanceBasePath = smootherBasePath; } if (smootherBasePath.EndPoint.Location.Length > 80) { return OnDynamicallyInfeasible(null, null); } Services.UIService.PushRelativePath(smootherBasePath, curTimestamp, "subpath2"); IList<Obstacle> obstacles = null; if (doAvoidance && Settings.DoAvoidance) { // get the obstacles predicted to the current timestamp ObstacleCollection obs = Services.ObstaclePipeline.GetProcessedObstacles(curTimestamp, Services.BehaviorManager.SAUDILevel); if (obs != null) obstacles = obs.obstacles; } if (extraObstacles != null && extraObstacles.Count > 0) { if (obstacles == null) { obstacles = extraObstacles; } else { foreach (Obstacle obs in extraObstacles) { obstacles.Add(obs); } } } // start the planning timer Stopwatch planningTimer = Stopwatch.StartNew(); List<Boundary> leftSmootherBounds = new List<Boundary>(); List<Boundary> rightSmootherBounds = new List<Boundary>(); leftSmootherBounds.AddRange(leftLaneBounds); rightSmootherBounds.AddRange(rightLaneBounds); leftSmootherBounds.AddRange(additionalLeftBounds); rightSmootherBounds.AddRange(additionalRightBounds); BlockageInstructions blockageInstructions = new BlockageInstructions(); blockageInstructions.smootherInstructions = SmootherInstructions.RunNormalSmoothing; bool pathBlocked = false; // check if there are any obstacles if (obstacles != null && obstacles.Count > 0) { // label the obstacles as ignored if (ignorableObstacles != null) { if (ignorableObstacles.Count == 1 && ignorableObstacles[0] == -1) { for (int i = 0; i < obstacles.Count; i++) { if (obstacles[i].obstacleClass == ObstacleClass.DynamicCarlike || obstacles[i].obstacleClass == ObstacleClass.DynamicStopped) { obstacles[i].ignored = true; } } } else { ignorableObstacles.Sort(); for (int i = 0; i < obstacles.Count; i++) { if (obstacles[i].trackID != -1 && ignorableObstacles.BinarySearch(obstacles[i].trackID) >= 0) { obstacles[i].ignored = true; } } } } // we need to do the full obstacle avoidance // execute the obstacle manager LinePath avoidancePath; bool success; List<LinePath> shiftedLeftBounds = new List<LinePath>(leftLaneBounds.Count); foreach (Boundary bnd in leftLaneBounds) { LinePath lp = (LinePath)bnd.Coords; if (lp.Count >= 2) { shiftedLeftBounds.Add(lp.ShiftLateral(-(TahoeParams.T)/2.0+0.25)); } } List<LinePath> shiftedRightBounds = new List<LinePath>(rightLaneBounds.Count); foreach (Boundary bnd in rightLaneBounds) { LinePath lp = (LinePath)bnd.Coords; if (lp.Count >= 2) { shiftedRightBounds.Add(lp.ShiftLateral((TahoeParams.T)/2.0-0.25)); } } try { Services.ObstacleManager.ProcessObstacles(avoidanceBasePath, shiftedLeftBounds, shiftedRightBounds, obstacles, laneWidthAtPathEnd, reverseGear, sparse, out avoidancePath, out success); } catch (OutOfMemoryException ex) { Console.WriteLine("out of memory exception at " + ex.StackTrace); return OnDynamicallyInfeasible(obstacles, null); } if (!success) { // build out the distance DetermineBlockageDistancesAndDeceleration(obstacles, avoidanceBasePath); // call the on blocked stuff blockageInstructions = OnPathBlocked(obstacles); pathBlocked = blockageInstructions.pathBlocked; } if (blockageInstructions.smootherInstructions == SmootherInstructions.RunNormalSmoothing || blockageInstructions.smootherInstructions == SmootherInstructions.UseSmootherSpeedCommands) { // build the boundary lists // sort out obstacles as left and right double obstacleAlphaS = 100; int totalObstacleClusters = obstacles.Count; for (int i = 0; i < totalObstacleClusters; i++) { if (obstacles[i].ignored) continue; double minSpacing = Math.Max(obstacles[i].minSpacing + smootherSpacingAdjust, 0); double desSpacing = Math.Max(obstacles[i].desSpacing + smootherSpacingAdjust, 0); if (obstacles[i].avoidanceStatus == AvoidanceStatus.Left) { Boundary bound = new Boundary(obstacles[i].AvoidancePolygon, minSpacing, desSpacing, obstacleAlphaS); bound.CheckFrontBumper = true; leftSmootherBounds.Add(bound); } else if (obstacles[i].avoidanceStatus == AvoidanceStatus.Right) { Boundary bound = new Boundary(obstacles[i].AvoidancePolygon, minSpacing, desSpacing, obstacleAlphaS); bound.CheckFrontBumper = true; rightSmootherBounds.Add(bound); } } } // we could possibly replace the smoother base with the avoidance path or we can adjust the smoother base path // appropriately if (success && useAvoidancePath) { smootherBasePath = avoidancePath; } Services.UIService.PushLineList(avoidancePath, curTimestamp, "avoidance path", true); } PlanningResult planningResult = null; if (blockageInstructions.smootherInstructions == SmootherInstructions.RunNormalSmoothing || blockageInstructions.smootherInstructions == SmootherInstructions.UseSmootherSpeedCommands || (blockageInstructions.smootherInstructions == SmootherInstructions.UsePreviousPath && prevSmoothedPath == null)) { // initialize settings that we're making easier for the derived classes settings.basePath = smootherBasePath; settings.targetPaths = smootherTargetPaths; settings.leftBounds = leftSmootherBounds; settings.rightBounds = rightSmootherBounds; settings.startSpeed = curSpeed; settings.timestamp = curTimestamp; PathPlanner.SmoothingResult result = planner.PlanPath(settings); if (result.result != SmoothResult.Sucess) { planningResult = OnDynamicallyInfeasible(obstacles, result.details); if (blockageInstructions.speedCommand != null) { planningResult.speedCommandGenerator = blockageInstructions.speedCommand; } } else { // build out the command planningResult = new PlanningResult(); planningResult.pathBlocked = pathBlocked; planningResult.smoothedPath = result.path; if (blockageInstructions.smootherInstructions == SmootherInstructions.UseSmootherSpeedCommands) { planningResult.speedCommandGenerator = new FeedbackSpeedCommandGenerator(result.path); } else if (blockageInstructions.speedCommand != null) { planningResult.speedCommandGenerator = blockageInstructions.speedCommand; } if (blockageInstructions.smootherInstructions == SmootherInstructions.UseCommandGenerator) { planningResult.steeringCommandGenerator = blockageInstructions.steeringCommand; } else { planningResult.steeringCommandGenerator = new PathSteeringCommandGenerator(result.path); } } } else if (blockageInstructions.smootherInstructions == SmootherInstructions.UsePreviousPath){ // transform the previously smoothed path into the current interval RelativeTransform transform = Services.RelativePose.GetTransform(prevSmoothedPathTimestamp, curTimestamp); SmoothedPath prevPath = new SmoothedPath(curTimestamp, prevSmoothedPath.Transform(transform), null); planningResult = new PlanningResult(); planningResult.speedCommandGenerator = blockageInstructions.speedCommand; planningResult.smoothedPath = prevPath; planningResult.pathBlocked = pathBlocked; planningResult.steeringCommandGenerator = new PathSteeringCommandGenerator(prevPath); } else if (blockageInstructions.smootherInstructions == SmootherInstructions.UseCommandGenerator) { planningResult = new PlanningResult(); planningResult.speedCommandGenerator = blockageInstructions.speedCommand; planningResult.steeringCommandGenerator = blockageInstructions.steeringCommand; planningResult.pathBlocked = pathBlocked; planningResult.smoothedPath = null; } planningTimer.Stop(); BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "planning took {0} ms", planningTimer.ElapsedMilliseconds); planningResult.commandLabel = blockageInstructions.commandLabel; if (!planningResult.pathBlocked && !planningResult.dynamicallyInfeasible) { OnSmoothSuccess(ref planningResult); } return planningResult; }
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; }
private double PostProcessSpeed(SmoothedPath path) { const double a_lat_max = 1.5; const double lat_target_decel = 1; double maxSpeed = 20; // return an astronomical speed if there are no curvatures if (path.Count < 3) return maxSpeed; // calculate the curvature at each point // we want the lateral acceleration to be no more than a_lat_max, so calculate the speed we would need to be going now achieve that deceleration double dist = 0; for (int i = 1; i < path.Count-1; i++) { dist += path[i-1].DistanceTo(path[i]); // lateral acceleration = v^2*curvature double curvature = path.GetCurvature(i); double desiredSpeed = Math.Sqrt(a_lat_max/Math.Abs(curvature)); double desiredMaxSpeed = Math.Sqrt(desiredSpeed*desiredSpeed + 2*lat_target_decel*dist); if (desiredMaxSpeed < maxSpeed) { maxSpeed = desiredMaxSpeed; } } return maxSpeed; }