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 BlockageInstructions OnPathBlocked(IList<Obstacle> obstacles, bool sendBlockage) { BlockageInstructions inst = new BlockageInstructions(); inst.smootherInstructions = SmootherInstructions.RunNormalSmoothing; inst.pathBlocked = true; // look at all the obstacles and see if we want to ignore any // for now, just figure out the worst-case distance double staticStopDist = double.MaxValue; double dynamicStopDist = double.MaxValue; bool dynamicIsStopped = false; double dynamicStopTol = stopped_spacing_dist; bool dynamicIsIgnored = false; bool dynamicIsOccluded = false; bool staticIsLow = false; int stopTrackID = -1; foreach (Obstacle obs in obstacles) { if (obs.avoidanceStatus == AvoidanceStatus.Collision || obs.ignored) { bool isDynamic = (obs.obstacleClass == ObstacleClass.DynamicCarlike || obs.obstacleClass == ObstacleClass.DynamicStopped); if (isDynamic) { // handle dynamic obstacle specially // if the obstacle is not ignored or the obstacle is stopped if (obs.obstacleDistance < dynamicStopDist && obs.obstacleDistance > 0) { dynamicStopDist = obs.obstacleDistance; stopTrackID = obs.trackID; dynamicIsIgnored = obs.ignored; dynamicIsStopped = (obs.obstacleClass == ObstacleClass.DynamicStopped); dynamicIsOccluded = obs.occuluded; // TODO: can adjust stop spacing for intersections if (obs.ignored) dynamicStopTol = TahoeParams.VL; else dynamicStopTol = stopped_spacing_dist; } } else if (!isDynamic && obs.obstacleDistance < staticStopDist && obs.obstacleDistance > 0) { staticStopDist = obs.obstacleDistance; staticIsLow = obs.obstacleClass == ObstacleClass.StaticSmall; } // reset the avoidance status to ignore obs.avoidanceStatus = AvoidanceStatus.Ignore; } } // determine what the deal is bool stopIsDynamic; double stopDist = Math.Min(dynamicStopDist, staticStopDist); if (stopDist == double.MaxValue) { inst.pathBlocked = false; return inst; } // allow dynamic obstacles to be the stopping point if they are up to 1 m closer double targetDecel; if (dynamicStopDist - blockage_dynamic_tol < staticStopDist) { stopIsDynamic = true; targetDecel = 3; // if the dynamic obstacle is ignored, then just break out because we don't really care if (dynamicIsIgnored && !dynamicIsStopped) { inst.pathBlocked = false; return inst; } } else { targetDecel = 2; stopIsDynamic = false; stopTrackID = -1; } Services.Dataset.ItemAs<double>("blockage dist").Add(stopDist, curTimestamp); double origStopDist = stopDist; double speedCommandStopDist = GetSpeedCommandStopDistance(); if (!double.IsInfinity(speedCommandStopDist) && speedCommandStopDist < stopDist) { if (speedCommandStopDist < stopDist - 2) { // we don't want to do anything special inst.pathBlocked = false; return inst; } else { stopDist -= 2; } } else if (stopIsDynamic) { stopDist -= dynamicStopTol; } else if (staticIsLow) { stopDist -= 5; } else { stopDist -= stopped_spacing_dist; } if (stopDist < 0.01) { stopDist = 0.01; } double stopSpeed = Math.Sqrt(2*targetDecel*stopDist); double stopDecel = vs.speed*vs.speed/(2*stopDist); Services.Dataset.ItemAs<double>("stop target speed").Add(stopSpeed, curTimestamp); double scalarSpeed = -1; if (speedCommand is ScalarSpeedCommand) { scalarSpeed = ((ScalarSpeedCommand)speedCommand).Speed; if (scalarSpeed <= stopSpeed) { // don't need to report a blockage or do anything since arbiter is already going slow enough inst.pathBlocked = false; return inst; } } if ((origStopDist < 25 && vs.IsStopped && scalarSpeed != 0 && (!stopIsDynamic || InIntersection() || dynamicIsOccluded)) || Services.BehaviorManager.TestMode) { // check the timing if (Services.BehaviorManager.TestMode) { sendBlockage = true; } else if (sendBlockage) { if (lastBlockageTime == null || lastBlockageDynamic != stopIsDynamic) { lastBlockageTime = HighResDateTime.Now; lastBlockageDynamic = stopIsDynamic; sendBlockage = false; } else if ((HighResDateTime.Now - lastBlockageTime.Value) < TimeSpan.FromSeconds(2)) { sendBlockage = false; } } if (sendBlockage) { // send a blockage report to AI UrbanChallenge.Behaviors.CompletionReport.CompletionResult result = (stopDecel > 5) ? UrbanChallenge.Behaviors.CompletionReport.CompletionResult.InevitableDeath : UrbanChallenge.Behaviors.CompletionReport.CompletionResult.Stopped; bool stopTooClose = origStopDist < TahoeParams.VL; BlockageType type; if (stopIsDynamic) { if (dynamicIsStopped && dynamicIsOccluded) { type = BlockageType.Static; } else { type = BlockageType.Dynamic; } } else { type = BlockageType.Static; } CompletionReport report = new TrajectoryBlockedReport(result, origStopDist, type, stopTrackID, stopTooClose || DetermineReverseRecommended(obstacles), Services.BehaviorManager.CurrentBehaviorType); ForwardCompletionReport(report); } } else { // clear out the last blockage time lastBlockageTime = null; } // at this point we've decided to handle the blockage and stop for it // we want to consider stopping for this // calculate an approximate minimum stopping distance double minStoppingDist = vs.speed*vs.speed/10.0; // adjust the planning distance if (stopDist < minStoppingDist) { inst.smootherInstructions = SmootherInstructions.UsePreviousPath; } else { // adjust the smoother target path to be the adjusted planning distance length smootherBasePath = GetPathBlockedSubPath(smootherBasePath, stopDist); inst.smootherInstructions = SmootherInstructions.RunNormalSmoothing; } if (vs.IsStopped && speedCommand is ScalarSpeedCommand && ((ScalarSpeedCommand)speedCommand).Speed == 0) { // don't do anything inst.speedCommand = new ConstantSpeedCommandGenerator(0, TahoeParams.brake_hold); inst.steeringCommand = new ConstantSteeringCommandGenerator(null, null, false); inst.smootherInstructions = SmootherInstructions.UseCommandGenerator; } else if (Math.Abs(vs.speed) < 3.5 && stopDist < 7) { if (vs.IsStopped && stopDist < 0.5) { inst.speedCommand = new ConstantSpeedCommandGenerator(0, TahoeParams.brake_hold); inst.steeringCommand = new ConstantSteeringCommandGenerator(null, null, false); inst.smootherInstructions = SmootherInstructions.UseCommandGenerator; } else { if (approachSpeed == null) { approachSpeed = Math.Max(Math.Abs(vs.speed), 1.5); } // if the deceleration is less than some threshold, inst.speedCommand = new FeedbackSpeedCommandGenerator(new StopSpeedGenerator(new TravelledDistanceProvider(curTimestamp, stopDist), approachSpeed.Value)); } } else { approachSpeed = null; if (vs.speed - stopSpeed > 0.5) { // we're going too fast for the normal speed control to stop in time inst.speedCommand = new FeedbackSpeedCommandGenerator(new ConstantAccelSpeedGenerator(-stopDecel)); } else { inst.speedCommand = new FeedbackSpeedCommandGenerator(new ConstantSpeedGenerator(stopSpeed, null)); } } inst.commandLabel = "blockage stop"; return inst; }