/// <summary>
/// Maneuver for recovering from a lane blockage, used for lane changes as well
/// </summary>
/// <param name="lane"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicleSpeed"></param>
/// <param name="defcon"></param>
/// <param name="saudi"></param>
/// <returns></returns>
public Maneuver LaneRecoveryManeuver(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed,
INavigationalPlan plan, BlockageRecoveryState brs, bool failedVehiclesPersistentlyOnly, out bool waitForUTurn)
{
// get the blockage
ITacticalBlockage laneBlockageReport = brs.EncounteredState.TacticalBlockage;
// get the turn state
StayInLaneState sils = new StayInLaneState(lane, CoreCommon.CorePlanningState);
// set wait false
waitForUTurn = false;
#region Reverse
// check the state of the recovery for being the initial state
if (brs.Defcon == BlockageRecoveryDEFCON.INITIAL)
{
// determine if the reverse behavior is recommended
if (laneBlockageReport.BlockageReport.ReverseRecommended)
{
// notify
ArbiterOutput.Output("Initial encounter, reverse recommended, reversing");
// get reverse behavior
StayInLaneBehavior reverseRecovery = this.LaneReverseRecovery(lane, vehicleState, vehicleSpeed, brs.EncounteredState.TacticalBlockage.BlockageReport);
reverseRecovery.TimeStamp = vehicleState.Timestamp;
// get recovery state
BlockageRecoveryState brsT = new BlockageRecoveryState(
reverseRecovery, null, new StayInLaneState(lane, CoreCommon.CorePlanningState),
brs.Defcon = BlockageRecoveryDEFCON.REVERSE, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
brsT.CompletionState = brsT;
// reset blockages
BlockageHandler.SetDefaultBlockageCooldown();
// maneuver
return new Maneuver(reverseRecovery, brsT, TurnDecorators.HazardDecorator, vehicleState.Timestamp);
}
}
#endregion
#region uTurn
// check if uturn is available
ArbiterLane opp = this.tacticalUmbrella.roadTactical.forwardReasoning.GetClosestOpposing(lane, vehicleState);
// resoning
OpposingLateralReasoning olrTmp = new OpposingLateralReasoning(opp, SideObstacleSide.Driver);
if (opp != null && olrTmp.ExistsExactlyHere(vehicleState) && opp.IsInside(vehicleState.Front) && opp.LanePath().GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front) < TahoeParams.VL * 3.0)
{
// check possible to reach goal given block partition way
RoadPlan uTurnNavTest = CoreCommon.Navigation.PlanRoadOppositeWithoutPartition(
lane.GetClosestPartition(vehicleState.Front),
opp.GetClosestPartition(vehicleState.Front),
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId],
true,
vehicleState.Front,
true);
// flag to try the uturn
bool uturnTry = false;
// all polygons to include
List<Polygon> allPolys = BlockageTactical.AllForwardPolygons(vehicleState, failedVehiclesPersistentlyOnly);// .AllObstacleAndStoppedVehiclePolygons(vehicleState);
// test blockage takes up segment
double numLanesLeft = lane.NumberOfLanesLeft(vehicleState.Front, true);
double numLanesRight = lane.NumberOfLanesRight(vehicleState.Front, true);
LinePath leftBound = lane.LanePath().ShiftLateral((lane.Width * numLanesLeft) + (lane.Width / 2.0));
LinePath rightBound = lane.LanePath().ShiftLateral((lane.Width * numLanesRight) + (lane.Width / 2.0));
bool segmentBlockage = BlockageTester.TestBlockage(allPolys, leftBound, rightBound);
uturnTry = segmentBlockage;
// check if we should try the uturn
if(uturnTry)
{
// check uturn timer
if(uTurnTimer.ElapsedMilliseconds / 1000.0 > 2.0)
{
#region Determine Checkpoint
// check route feasible
if (uTurnNavTest.BestPlan.laneWaypointOfInterest.RouteTime < double.MaxValue - 1
&& uTurnNavTest.BestPlan.laneWaypointOfInterest.TimeCostToPoint < double.MaxValue - 1)
{
ArbiterOutput.Output("valid route to checkpoint by rerouting, uturning");
}
// otherwise remove the next checkpoint
else
{
// notiify
ArbiterOutput.Output("NO valid route to checkpoint by rerouting");
// get the next cp
ArbiterCheckpoint cp1 = CoreCommon.Mission.MissionCheckpoints.Peek();
// get distance to blockage
double blockageDistance = 15.0;
// check cp is in our lane
if (cp1.WaypointId.AreaSubtypeId.Equals(lane.LaneId))
{
// check distance to cp1
double distanceToCp1 = lane.DistanceBetween(vehicleState.Front, CoreCommon.RoadNetwork.ArbiterWaypoints[cp1.WaypointId].Position);
// check that this is an already inserted waypoint
if (cp1.Type == CheckpointType.Inserted)
{
// remove cp
ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as inserted type of checkpoint");
ArbiterCheckpoint ac2 = CoreCommon.Mission.MissionCheckpoints.Dequeue();
ArbiterOutput.Output("removed checkpoint: " + ac2.WaypointId.ToString() + " as blocked type of checkpoint");
}
// closer to us than the blockage
else if (distanceToCp1 < blockageDistance)
{
// remove cp
ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as between us and the blockage ~ 15m away");
}
// very close to blockage on the other side
else if (distanceToCp1 < blockageDistance + 5.0)
{
// remove cp
ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as on the other side of blockage ~ 15m away");
}
// further away from the blockage
else
{
// check over all checkpoints if there exists a checkpoint cp2 in the opposing lane within 5m along our lane
ArbiterCheckpoint cp2 = null;
foreach (ArbiterWaypoint oppAw in opp.WaypointList)
{
// check checkpoint
if (oppAw.IsCheckpoint)
{
// distance between us and that waypoint
double distanceToCp2 = lane.DistanceBetween(vehicleState.Front, oppAw.Position);
// check along distance < 5.0m
if (Math.Abs(distanceToCp1 - distanceToCp2) < 5.0)
{
// set cp
cp2 = new ArbiterCheckpoint(oppAw.CheckpointId, oppAw.WaypointId, CheckpointType.Inserted);
}
}
}
// check close cp exists
if (cp2 != null)
{
// remove cp1 and replace with cp2
ArbiterOutput.Output("inserting checkpoint: " + cp2.WaypointId.ToString() + " before checkpoint: " + cp1.WaypointId.ToString() + " as can be replaced with adjacent opposing cp2");
//CoreCommon.Mission.MissionCheckpoints.Dequeue();
// get current checkpoints
ArbiterCheckpoint[] updatedAcs = new ArbiterCheckpoint[CoreCommon.Mission.MissionCheckpoints.Count + 1];
updatedAcs[0] = cp2;
CoreCommon.Mission.MissionCheckpoints.CopyTo(updatedAcs, 1);
updatedAcs[1].Type = CheckpointType.Blocked;
CoreCommon.Mission.MissionCheckpoints = new Queue<ArbiterCheckpoint>(new List<ArbiterCheckpoint>(updatedAcs));
}
// otherwise remove cp1
else
{
// remove cp
ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as cp not further down our lane");
}
}
}
// otherwise we remove the checkpoint
else
{
// remove cp
ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as cp not further down our lane");
}
}
#endregion
#region Plan Uturn
// notify
ArbiterOutput.Output("Segment blocked, uturn available");
// nav penalties
ArbiterLanePartition alpClose = lane.GetClosestPartition(vehicleState.Front);
CoreCommon.Navigation.AddHarshBlockageAcrossSegment(alpClose, vehicleState.Front);
// uturn
LinePath lpTmp = new LinePath(new Coordinates[] { vehicleState.Front, opp.LanePath().GetClosestPoint(vehicleState.Front).Location });
Coordinates vector = lpTmp[1] - lpTmp[0];
lpTmp[1] = lpTmp[1] + vector.Normalize(opp.Width / 2.0);
lpTmp[0] = lpTmp[0] - vector.Normalize(lane.Width / 2.0);
LinePath lb = lpTmp.ShiftLateral(15.0);
LinePath rb = lpTmp.ShiftLateral(-15.0);
List<Coordinates> uturnPolyCOords = new List<Coordinates>();
uturnPolyCOords.AddRange(lb);
uturnPolyCOords.AddRange(rb);
uTurnState uts = new uTurnState(opp, Polygon.GrahamScan(uturnPolyCOords));
// reset blockages
BlockageHandler.SetDefaultBlockageCooldown();
// reset the timers
this.Reset();
// go to uturn
return new Maneuver(uts.Resume(vehicleState, 2.0), uts, TurnDecorators.NoDecorators, vehicleState.Timestamp);
#endregion
}
// uturn timer not enough
else
{
// check timer running
if(!uTurnTimer.IsRunning)
{
uTurnTimer.Stop();
uTurnTimer.Reset();
uTurnTimer.Start();
}
// if gets here, need to wait
double time = uTurnTimer.ElapsedMilliseconds / 1000.0;
ArbiterOutput.Output("uTurn behavior evaluated to success, cooling down for: " + time.ToString("f2") + " out of 2");
waitForUTurn = true;
return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
else
{
waitForUTurn = false;
this.Reset();
}
}
else
{
waitForUTurn = false;
this.Reset();
}
#endregion
#region Recovery Escalation
// plan forward reasoning
this.tacticalUmbrella.roadTactical.forwardReasoning.ForwardManeuver(lane, vehicleState, (RoadPlan)plan, new List<ITacticalBlockage>(), new List<ArbiterWaypoint>());
// check clear on right, or if it does nto exist here
ILateralReasoning rightLateral = this.tacticalUmbrella.roadTactical.rightLateralReasoning;
bool rightClear = !rightLateral.Exists || !rightLateral.ExistsExactlyHere(vehicleState) ||
rightLateral.AdjacentAndRearClear(vehicleState);
// check clear on left, or if it does nto exist here
ILateralReasoning leftLateral = this.tacticalUmbrella.roadTactical.leftLateralReasoning;
bool leftClear = !leftLateral.Exists || !leftLateral.ExistsExactlyHere(vehicleState) ||
leftLateral.AdjacentAndRearClear(vehicleState);
if(leftClear && leftLateral is OpposingLateralReasoning)
leftClear = leftLateral.ForwardClear(vehicleState, TahoeParams.VL * 3.0, 2.24,
new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, null),
lane.LanePath().AdvancePoint(lane.LanePath().GetClosestPoint(vehicleState.Front), TahoeParams.VL * 3.0).Location);
// check both clear to widen
bool widenOk = rightClear && leftClear;
// Notify
ArbiterOutput.Output("Blockage tactical recovery escalation: rightClear: " + rightClear.ToString() + ", leftCler: " + leftClear.ToString());
// if we can't widen for some reason just go through with no widen
StayInLaneBehavior silb = this.LaneRecoveryBehavior(lane, vehicleState, vehicleSpeed, plan, brs, brs.EncounteredState);
silb.TimeStamp = vehicleState.Timestamp;
// check widen
if (widenOk)
{
// output
ArbiterOutput.Output("Lane Blockage Recovery: Adjacent areas clear");
// mini icrease width
silb.LaneWidth = silb.LaneWidth + TahoeParams.T;
// check execute none saudi
CompletionReport l0Cr;
bool l0TestOk = CoreCommon.Communications.TestExecute(silb, out l0Cr);
// check mini ok
if (l0TestOk)
{
// notify
ArbiterOutput.Output("Lane Blockage Recovery: Test Tahoe.T lane widen ok");
// update the current state
BlockageRecoveryState brsL0 = new BlockageRecoveryState(
silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
return new Maneuver(silb, brsL0, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
{
// notify
ArbiterOutput.Output("Lane Blockage Recovery: Test Tahoe.T lane widen failed, moving to large widen");
#region Change Lanes
// check not in change lanes
if (brs.Defcon != BlockageRecoveryDEFCON.CHANGELANES_FORWARD && brs.Defcon != BlockageRecoveryDEFCON.WIDENBOUNDS)
{
// check normal change lanes reasoning
bool shouldWait;
IState laneChangeCompletionState;
ChangeLaneBehavior changeLanesBehavior = this.ChangeLanesRecoveryBehavior(lane, vehicleState, out shouldWait, out laneChangeCompletionState);
// check change lanes behaviore exists
if (changeLanesBehavior != null)
{
ArbiterOutput.Output("Lane Blockage Recovery: Found adjacent lane available change lanes beahvior: " + changeLanesBehavior.ToShortString() + ", " + changeLanesBehavior.ShortBehaviorInformation());
// update the current state
BlockageRecoveryState brsCL = new BlockageRecoveryState(
changeLanesBehavior, laneChangeCompletionState, sils, BlockageRecoveryDEFCON.CHANGELANES_FORWARD, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
return new Maneuver(changeLanesBehavior, brsCL, changeLanesBehavior.ChangeLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator, vehicleState.Timestamp);
}
// check should wait
else if (shouldWait)
{
ArbiterOutput.Output("Lane Blockage Recovery: Should wait for the forward lane, waiting");
return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
// notify
ArbiterOutput.Output("Lane Blockage Recovery: Fell Through Forward Change Lanes");
// increase width
silb.LaneWidth = silb.LaneWidth * 3.0;
// check execute l1
CompletionReport l1Cr;
bool l1TestOk = CoreCommon.Communications.TestExecute(silb, out l1Cr);
// check ok
if (l1TestOk)
{
// notify
ArbiterOutput.Output("Lane Blockage Recovery: Test 3LW lane large widen ok");
// update the current state
BlockageRecoveryState brsL1 = new BlockageRecoveryState(
silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
return new Maneuver(silb, brsL1, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// go to l2 for all the marbles
else
{
// notify
ArbiterOutput.Output("Lane Blockage Recovery: Test 3LW lane large widen failed, moving to 3LW, L1 Saudi");
ShutUpAndDoItDecorator saudi2 = new ShutUpAndDoItDecorator(SAUDILevel.L1);
List<BehaviorDecorator> saudi2bds = new List<BehaviorDecorator>(new BehaviorDecorator[] { saudi2 });
silb.Decorators = saudi2bds;
BlockageRecoveryState brsL2 = new BlockageRecoveryState(
silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
return new Maneuver(silb, brsL2, saudi2bds, vehicleState.Timestamp);
}
}
}
#endregion
// fallout goes back to lane state
return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
/// <summary>
/// Maneuver for recovering from a lane blockage, used for lane changes as well
/// </summary>
/// <param name="lane"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicleSpeed"></param>
/// <param name="defcon"></param>
/// <param name="saudi"></param>
/// <returns></returns>
public Maneuver OpposingLaneRecoveryManeuver(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed,
INavigationalPlan plan, BlockageRecoveryState brs, bool failedVehiclesPersistentlyOnly)
{
// get the blockage
ITacticalBlockage laneBlockageReport = brs.EncounteredState.TacticalBlockage;
// get the turn state
OpposingLanesState sils = (OpposingLanesState)brs.EncounteredState.PlanningState;
#region Reverse
// check the state of the recovery for being the initial state
if (brs.Defcon == BlockageRecoveryDEFCON.INITIAL)
{
// determine if the reverse behavior is recommended
if (laneBlockageReport.BlockageReport.ReverseRecommended)
{
// notify
ArbiterOutput.Output("Initial encounter, reverse recommended, reversing");
// path
LinePath revPath = lane.LanePath().Clone();
revPath.Reverse();
// dist to reverse
double distToReverse;
if (double.IsNaN(laneBlockageReport.BlockageReport.DistanceToBlockage))
distToReverse = TahoeParams.VL * 1.5;
else if (laneBlockageReport.BlockageReport.DistanceToBlockage < 0 || laneBlockageReport.BlockageReport.DistanceToBlockage > 2 * TahoeParams.VL)
distToReverse = TahoeParams.VL * 1.5;
else
distToReverse = TahoeParams.VL - laneBlockageReport.BlockageReport.DistanceToBlockage;
// get reverse behavior
StayInLaneBehavior reverseRecovery = new StayInLaneBehavior(lane.LaneId,
new StopAtDistSpeedCommand(TahoeParams.VL * 2.0, true), new List<int>(),
revPath, lane.Width, lane.NumberOfLanesLeft(vehicleState.Front, false), lane.NumberOfLanesRight(vehicleState.Front, false));
// get recovery state
BlockageRecoveryState brsT = new BlockageRecoveryState(
reverseRecovery, CoreCommon.CorePlanningState, new OpposingLanesState(lane, false, CoreCommon.CorePlanningState, vehicleState),
brs.Defcon = BlockageRecoveryDEFCON.REVERSE, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
brsT.CompletionState = brsT;
// reset blockages
BlockageHandler.SetDefaultBlockageCooldown();
// maneuver
return new Maneuver(reverseRecovery, brsT, TurnDecorators.HazardDecorator, vehicleState.Timestamp);
}
}
#endregion
#region Recovery Escalation
// plan forward reasoning
this.tacticalUmbrella.roadTactical.opposingReasoning.ForwardManeuver(lane,
((OpposingLanesState)brs.EncounteredState.PlanningState).ClosestGoodLane,
vehicleState, (RoadPlan)plan, new List<ITacticalBlockage>());
// check clear on right, or if it does nto exist here
ILateralReasoning rightLateral = this.tacticalUmbrella.roadTactical.rightLateralReasoning;
bool rightClear = !rightLateral.Exists || !rightLateral.ExistsExactlyHere(vehicleState) ||
rightLateral.AdjacentAndRearClear(vehicleState);
// check clear on left, or if it does nto exist here
ILateralReasoning leftLateral = this.tacticalUmbrella.roadTactical.leftLateralReasoning;
bool leftClear = !leftLateral.Exists || !leftLateral.ExistsExactlyHere(vehicleState) ||
leftLateral.AdjacentAndRearClear(vehicleState);
if (leftClear && leftLateral is OpposingLateralReasoning)
leftClear = leftLateral.ForwardClear(vehicleState, TahoeParams.VL * 3.0, 2.24,
new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, null),
lane.LanePath().AdvancePoint(lane.LanePath().GetClosestPoint(vehicleState.Front), TahoeParams.VL * 3.0).Location);
// check both clear to widen
bool widenOk = rightClear && leftClear;
// Notify
ArbiterOutput.Output("Blockage tactical recovery escalation: rightClear: " + rightClear.ToString() + ", leftClear: " + leftClear.ToString());
// path
LinePath revPath2 = lane.LanePath().Clone();
revPath2.Reverse();
// check widen
if (widenOk)
{
// output
ArbiterOutput.Output("Lane Blockage Recovery: Adjacent areas clear");
if (brs.Defcon != BlockageRecoveryDEFCON.CHANGELANES_FORWARD)
{
// check change lanes back to good
ChangeLaneBehavior clb = new ChangeLaneBehavior(
lane.LaneId, sils.ClosestGoodLane.LaneId, false, TahoeParams.VL * 3.0, new StopAtDistSpeedCommand(TahoeParams.VL * 3.0), new List<int>(),
revPath2, sils.ClosestGoodLane.LanePath(), lane.Width, sils.ClosestGoodLane.Width,
lane.NumberOfLanesLeft(vehicleState.Front, false), lane.NumberOfLanesRight(vehicleState.Front, false));
// change lanes
brs.Defcon = BlockageRecoveryDEFCON.CHANGELANES_FORWARD;
// cehck ok
CompletionReport clROk;
bool clCheck = CoreCommon.Communications.TestExecute(clb, out clROk);
// check change lanes ok
if (clCheck)
{
ArbiterOutput.Output("Change lanes behaviro ok, executing");
// return manevuer
return new Maneuver(clb, brs, TurnDecorators.RightTurnDecorator, vehicleState.Timestamp);
}
else
{
ArbiterOutput.Output("Change lanes behaviro not ok");
}
}
// if we can't widen for some reason just go through with no widen
StayInLaneBehavior silb = new StayInLaneBehavior(lane.LaneId,
new StopAtDistSpeedCommand(TahoeParams.VL * 2.0), new List<int>(),
revPath2,
lane.Width, lane.NumberOfLanesLeft(vehicleState.Front, false), lane.NumberOfLanesRight(vehicleState.Front, false));
// mini icrease width
silb.LaneWidth = silb.LaneWidth + TahoeParams.T;
// check execute none saudi
CompletionReport l0Cr;
bool l0TestOk = CoreCommon.Communications.TestExecute(silb, out l0Cr);
// check mini ok
if (l0TestOk)
{
// notify
ArbiterOutput.Output("Lane Blockage Recovery: Test Tahoe.T lane widen ok");
// update the current state
BlockageRecoveryState brsL0 = new BlockageRecoveryState(
silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
return new Maneuver(silb, brsL0, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
{
// notify
ArbiterOutput.Output("Lane Blockage Recovery: Test Tahoe.T lane widen failed, moving to large widen");
// increase width
silb.LaneWidth = silb.LaneWidth * 3.0;
// check execute l1
CompletionReport l1Cr;
bool l1TestOk = CoreCommon.Communications.TestExecute(silb, out l1Cr);
// check ok
if (l1TestOk)
{
// notify
ArbiterOutput.Output("Lane Blockage Recovery: Test 3LW lane large widen ok");
// update the current state
BlockageRecoveryState brsL1 = new BlockageRecoveryState(
silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
return new Maneuver(silb, brsL1, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// go to l2 for all the marbles
else
{
// notify
ArbiterOutput.Output("Lane Blockage Recovery: Test 3LW lane large widen failed, moving to 3LW, L1 Saudi");
ShutUpAndDoItDecorator saudi2 = new ShutUpAndDoItDecorator(SAUDILevel.L1);
List<BehaviorDecorator> saudi2bds = new List<BehaviorDecorator>(new BehaviorDecorator[] { saudi2 });
silb.Decorators = saudi2bds;
BlockageRecoveryState brsL2 = new BlockageRecoveryState(
silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
// check execute l1
CompletionReport l2Cr;
bool l2TestOk = CoreCommon.Communications.TestExecute(silb, out l1Cr);
// go
return new Maneuver(silb, brsL2, saudi2bds, vehicleState.Timestamp);
}
}
}
#endregion
// fallout goes back to lane state
return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
/// <summary>
/// Maneuver for recovering from a turn blockage
/// </summary>
/// <param name="lane"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicleSpeed"></param>
/// <param name="defcon"></param>
/// <param name="saudi"></param>
/// <returns></returns>
private Maneuver TurnRecoveryManeuver(ArbiterInterconnect ai, VehicleState vehicleState,
double vehicleSpeed, BlockageRecoveryState brs)
{
// get the blockage
ITacticalBlockage turnBlockageReport = brs.EncounteredState.TacticalBlockage;
// get the turn state
TurnState turnState = (TurnState)brs.EncounteredState.PlanningState;
// check the state of the recovery for being the initial state
if (brs.Defcon == BlockageRecoveryDEFCON.INITIAL)
{
// determine if the reverse behavior is recommended
if (turnBlockageReport.BlockageReport.ReverseRecommended)
{
// get the path of the interconnect
LinePath interconnectPath = ai.InterconnectPath;
// check how much room there is to the start of the interconnect
double distanceToStart = interconnectPath.DistanceBetween(interconnectPath.StartPoint, interconnectPath.GetClosestPoint(vehicleState.Rear));
// check distance to start from the rear bumper is large enough
if (distanceToStart > 0)
{
// notify
ArbiterOutput.Output("Initial encounter of turn blockage with reverse recommended, reversing");
// get hte reverse path
LinePath reversePath = vehicleState.VehicleLinePath;
// generate the reverse recovery behavior
StopAtDistSpeedCommand sadsc = new StopAtDistSpeedCommand(TahoeParams.VL, true);
StayInLaneBehavior silb = new StayInLaneBehavior(null, sadsc, new List<int>(), reversePath, TahoeParams.T * 2.0, 0, 0);
// update the state
BlockageRecoveryState brsUpdated = new BlockageRecoveryState(
silb, brs.CompletionState, brs.AbortState, BlockageRecoveryDEFCON.REVERSE,
brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
// chill out
BlockageHandler.SetDefaultBlockageCooldown();
// send the maneuver
Maneuver recoveryManeuver = new Maneuver(
silb, brsUpdated, TurnDecorators.HazardDecorator, vehicleState.Timestamp);
return recoveryManeuver;
}
}
}
// get the default turn behavior
TurnBehavior defaultTurnBehavior = (TurnBehavior)turnState.Resume(vehicleState, vehicleSpeed);
// check that we are not already ignoring small obstacles
if (turnState.Saudi == SAUDILevel.None)
{
// check the turn ignoring small obstacles
ShutUpAndDoItDecorator saudiDecorator = new ShutUpAndDoItDecorator(SAUDILevel.L1);
defaultTurnBehavior.Decorators.Add(saudiDecorator);
turnState.Saudi = SAUDILevel.L1;
// notify
ArbiterOutput.Output("Attempting test of turn behavior ignoring small obstacles");
// test execute the turn
CompletionReport saudiCompletionReport;
bool completedSaudiTurn = CoreCommon.Communications.TestExecute(defaultTurnBehavior, out saudiCompletionReport);
// if the turn worked with ignorimg small obstacles, execute that
if (completedSaudiTurn)
{
// notify
ArbiterOutput.Output("Test of turn behavior ignoring small obstacles successful");
// chill out
BlockageHandler.SetDefaultBlockageCooldown();
// return the recovery maneuver
return new Maneuver(defaultTurnBehavior, turnState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
// notify
ArbiterOutput.Output("Turn behavior reached last level of using no turn boundaries");
// when ignoring small obstacles does not work, send the turn without boundaries and ignore small obstacles
turnState.UseTurnBounds = false;
turnState.LeftBound = null;
turnState.RightBound = null;
defaultTurnBehavior.RightBound = null;
defaultTurnBehavior.LeftBound = null;
// ignoring small obstacles
ShutUpAndDoItDecorator saudiNoBoundsDecorator = new ShutUpAndDoItDecorator(SAUDILevel.L1);
defaultTurnBehavior.Decorators.Add(saudiNoBoundsDecorator);
turnState.Saudi = SAUDILevel.L1;
// chill out
BlockageHandler.SetDefaultBlockageCooldown();
// go to the turn state
return new Maneuver(defaultTurnBehavior, turnState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}