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