/// <summary> /// Constructor /// </summary> public BlockageHandler() { this.recentReport = null; this.timeoutTimer = new Stopwatch(); cooldownTimer = new Stopwatch(); }
/// <summary> /// sets the current blockage report /// </summary> /// <param name="tbr"></param> public void OnBlockageReport(TrajectoryBlockedReport tbr) { // set the report this.recentReport = tbr; // output ArbiterOutput.Output("Blockage report: Behavior: " + tbr.BehaviorType.ToString() + ", Type: " + tbr.BlockageType.ToString() + ", Dist: " + tbr.DistanceToBlockage.ToString("f2") + ", Result: " + tbr.Result.ToString() + ", Reverse Rec: " + tbr.ReverseRecommended.ToString() + ", Saudi: " + tbr.SAUDILevel.ToString() + ", Track: " + tbr.TrackID.ToString() + "\n"); // lock the timer lock (timeoutTimer) { this.timeoutTimer.Stop(); this.timeoutTimer.Reset(); this.timeoutTimer.Start(); } }
/// <summary> /// Resets current report /// </summary> public void Reset() { // prevent overwrites lock (timeoutTimer) { this.recentReport = null; this.timeoutTimer.Stop(); this.timeoutTimer.Reset(); } }
/// <summary> /// Reverse in the lane /// </summary> /// <param name="lane"></param> /// <param name="vehicleState"></param> /// <param name="vehicleSpeed"></param> /// <returns></returns> private StayInLaneBehavior LaneReverseRecovery(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed, TrajectoryBlockedReport blockage) { // distance to reverse double distToReverse = double.IsNaN(blockage.DistanceToBlockage) ? TahoeParams.VL * 2.0 : TahoeParams.VL - blockage.DistanceToBlockage; if (distToReverse <= 3.0) distToReverse = TahoeParams.VL; // just reverse and let brian catch it StayInLaneBehavior reverseBehavior = new StayInLaneBehavior( lane.LaneId, new StopAtDistSpeedCommand(distToReverse, true), new List<int>(), lane.LanePath(), lane.Width, lane.NumberOfLanesLeft(vehicleState.Front, true), lane.NumberOfLanesRight(vehicleState.Front, true)); #region Start too close to start of lane // check distance to the start of the lane double laneStartDistanceFromFront = lane.DistanceBetween(lane.WaypointList[0].Position, vehicleState.Front); if (laneStartDistanceFromFront < TahoeParams.VL) { // make sure we're not at the very start of the lane if (laneStartDistanceFromFront < 0.5) { // do nothing return null; } // otherwise back up to the start of the lane else { // output ArbiterOutput.Output("Too close to the start of the lane, setting reverse distance to TP.VL"); // set proper distance distToReverse = TahoeParams.VL; // set behavior speed (no car behind us as too close to start of lane) LinePath bp = vehicleState.VehicleLinePath; reverseBehavior.SpeedCommand = new StopAtDistSpeedCommand(distToReverse, true); StayInLaneBehavior silb = new StayInLaneBehavior(null, reverseBehavior.SpeedCommand, new List<int>(), bp, lane.Width * 1.5, 0, 0); return silb; } } #endregion #region Sparse // check distance to the start of the lane if (lane.GetClosestPartition(vehicleState.Front).Type == PartitionType.Sparse) { // set behavior speed (no car behind us as too close to start of lane) LinePath bp = vehicleState.VehicleLinePath; reverseBehavior.SpeedCommand = new StopAtDistSpeedCommand(distToReverse, true); StayInLaneBehavior silb = new StayInLaneBehavior(null, reverseBehavior.SpeedCommand, new List<int>(), bp, lane.Width * 1.5, 0, 0); return silb; } #endregion #region Vehicle Behind us // rear monitor RearQuadrantMonitor rearMonitor = this.tacticalUmbrella.roadTactical.forwardReasoning.RearMonitor; // update and check for vehicle rearMonitor.Update(vehicleState); if (rearMonitor.CurrentVehicle != null) { // check for not failed forward vehicle if (rearMonitor.CurrentVehicle.QueuingState.Queuing != QueuingState.Failed) { // check if enough room to rear vehicle double vehicleDistance = lane.DistanceBetween(rearMonitor.CurrentVehicle.ClosestPosition, vehicleState.Rear); if (vehicleDistance < distToReverse - 2.0) { // revised distance given vehicle double revisedDistance = vehicleDistance - 2.0; // check enough room if (revisedDistance > TahoeParams.VL) { // set the updated speed command reverseBehavior.SpeedCommand = new StopAtDistSpeedCommand(revisedDistance, true); } // not enough room else { // output not enough room because of vehicle ArbiterOutput.Output("Blockage recovery, not enough room in rear because of rear vehicle, waiting for it to clear"); return null; } } } } #endregion // all clear, return reverse maneuver, set cooldown return reverseBehavior; }
/// <summary> /// Constructor /// </summary> /// <param name="blockageReport"></param> public LaneBlockage(TrajectoryBlockedReport blockageReport) { this.blockageReport = blockageReport; }
/// <summary> /// Constructor /// </summary> /// <param name="blockageReport"></param> public BlockageRecoveryBlockage(TrajectoryBlockedReport blockageReport) { this.blockageReport = blockageReport; }
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; }
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; }