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