public override bool Equals(object obj)
{
if (obj is StayInLaneBehavior)
{
StayInLaneBehavior b = (StayInLaneBehavior)obj;
return(lane.Equals(b.lane) && speedCommand.Equals(b.speedCommand));
}
else
{
return(false);
}
}
/// <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);
}
/// <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>
/// Reverse because of a blockage
/// </summary>
/// <param name="lane"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicleSpeed"></param>
/// <param name="defcon"></param>
/// <param name="saudi"></param>
/// <param name="laneOpposing"></param>
/// <param name="currentBlockage"></param>
/// <param name="ebs"></param>
/// <returns></returns>
private Maneuver LaneReverseManeuver(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed,
BlockageRecoveryDEFCON defcon, SAUDILevel saudi, bool laneOpposing, ITacticalBlockage currentBlockage, EncounteredBlockageState ebs)
{
// distance to reverse
double distToReverse = TahoeParams.VL * 2.0;
// 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));
// get the saudi level
List<BehaviorDecorator> decs = new List<BehaviorDecorator>(TurnDecorators.HazardDecorator.ToArray());
decs.Add(new ShutUpAndDoItDecorator(saudi));
reverseBehavior.Decorators = decs;
// state
IState laneState = new StayInLaneState(lane, CoreCommon.CorePlanningState);
BlockageRecoveryState brs = new BlockageRecoveryState(
reverseBehavior, laneState, laneState, defcon, ebs, BlockageRecoverySTATUS.EXECUTING);
// check enough room in lane to reverse
RearQuadrantMonitor rearMonitor = this.tacticalUmbrella.roadTactical.forwardReasoning.RearMonitor;
if (rearMonitor == null || !rearMonitor.lane.Equals(lane))
this.tacticalUmbrella.roadTactical.forwardReasoning.RearMonitor = new RearQuadrantMonitor(lane, SideObstacleSide.Driver);
#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)
{
// output
ArbiterOutput.Output("Too close to the start of the lane, raising defcon");
// go up chain
return new Maneuver(new NullBehavior(),
new EncounteredBlockageState(currentBlockage, laneState, BlockageRecoveryDEFCON.WIDENBOUNDS, saudi),
TurnDecorators.NoDecorators,
vehicleState.Timestamp);
}
// 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 new Maneuver(silb, brs, decs, vehicleState.Timestamp);
}
}
#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 new Maneuver(silb, brs, decs, vehicleState.Timestamp);
}
#endregion
#region Vehicle Behind us
// 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 new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
}
// check failed rear vehicle, up defcon
else
{
// failed vehicle in rear, go up chain
return new Maneuver(new NullBehavior(),
new EncounteredBlockageState(currentBlockage, laneState, BlockageRecoveryDEFCON.WIDENBOUNDS, saudi),
TurnDecorators.NoDecorators,
vehicleState.Timestamp);
}
}
#endregion
// all clear, return reverse maneuver, set cooldown
BlockageHandler.SetDefaultBlockageCooldown();
return new Maneuver(reverseBehavior, brs, decs, vehicleState.Timestamp);
}
/// <summary>
/// Get the default recovery behavior
/// </summary>
/// <param name="lane"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicleSpeed"></param>
/// <param name="brs"></param>
/// <param name="ebs"></param>
/// <returns></returns>
private StayInLaneBehavior LaneRecoveryBehavior(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed, INavigationalPlan plan,
BlockageRecoveryState brs, EncounteredBlockageState ebs)
{
#region Get the Recovery Behavior
// set the default distance to go forwards
double distanceForwards = TahoeParams.VL * 3.0;
// check distance to next lane point
this.tacticalUmbrella.roadTactical.forwardReasoning.ForwardMonitor.Primary(lane, vehicleState, (RoadPlan)plan, new List<ITacticalBlockage>() , new List<ArbiterWaypoint>(), false);
// get navigation distance to go
double navDistanceToGo = this.tacticalUmbrella.roadTactical.forwardReasoning.ForwardMonitor.NavigationParameters.DistanceToGo;
distanceForwards = navDistanceToGo < distanceForwards ? navDistanceToGo : distanceForwards;
// check if there is a forward vehicle we should follow normally
if (this.tacticalUmbrella.roadTactical.forwardReasoning.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker)
{
// fv distance
double fvDistance = this.tacticalUmbrella.roadTactical.forwardReasoning.ForwardMonitor.ForwardVehicle.ForwardControl.xSeparation;
// check distance forwards to vehicle
if (fvDistance < distanceForwards)
{
// distance modification
distanceForwards = fvDistance > 2.0 ? fvDistance - 2.0 : fvDistance;
}
}
// test behavior
StayInLaneBehavior testForwards = new StayInLaneBehavior(
lane.LaneId, new StopAtDistSpeedCommand(distanceForwards), new List<int>(), lane.LanePath(), lane.Width,
lane.NumberOfLanesLeft(vehicleState.Front, true), lane.NumberOfLanesRight(vehicleState.Front, true));
// set the specifiec saudi level
testForwards.Decorators = new List<BehaviorDecorator>(new BehaviorDecorator[] { new ShutUpAndDoItDecorator(brs.EncounteredState.Saudi) });
// return the test
return testForwards;
#endregion
}
/// <summary>
/// Parameters to follow the forward vehicle
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
/// <returns></returns>
public TravelingParameters Follow(IFQMPlanable lane, VehicleState state, List<ArbiterWaypoint> ignorable)
{
// travelling parameters
TravelingParameters tp = new TravelingParameters();
// get control parameters
ForwardVehicleTrackingControl fvtc = GetControl(lane, state, ignorable);
this.ForwardControl = fvtc;
// initialize the parameters
tp.DistanceToGo = fvtc.xDistanceToGood;
tp.NextState = CoreCommon.CorePlanningState;
tp.RecommendedSpeed = fvtc.vFollowing;
tp.Type = TravellingType.Vehicle;
tp.Decorators = new List<BehaviorDecorator>();
// ignore the forward vehicles
tp.VehiclesToIgnore = this.VehiclesToIgnore;
#region Following Control
#region Immediate Stop
// need to stop immediately
if (fvtc.vFollowing == 0.0)
{
// speed command
SpeedCommand sc = new ScalarSpeedCommand(0.0);
tp.SpeedCommand = sc;
tp.UsingSpeed = true;
if (lane is ArbiterLane)
{
// standard path following behavior
ArbiterLane al = ((ArbiterLane)lane);
Behavior final = new StayInLaneBehavior(al.LaneId, sc, this.VehiclesToIgnore, al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true));
final.Decorators = tp.Decorators;
tp.Behavior = final;
}
else
{
SupraLane sl = (SupraLane)lane;
StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState;
Behavior final = sisls.GetBehavior(sc, state.Front, this.VehiclesToIgnore);
final.Decorators = tp.Decorators;
tp.Behavior = final;
}
}
#endregion
#region Stopping at Distance
// stop at distance
else if (fvtc.vFollowing < 0.7 &&
CoreCommon.Communications.GetVehicleSpeed().Value <= 2.24 &&
fvtc.xSeparation > fvtc.xAbsMin)
{
// speed command
SpeedCommand sc = new StopAtDistSpeedCommand(fvtc.xDistanceToGood);
tp.SpeedCommand = sc;
tp.UsingSpeed = false;
if (lane is ArbiterLane)
{
ArbiterLane al = (ArbiterLane)lane;
// standard path following behavior
Behavior final = new StayInLaneBehavior(al.LaneId, sc, this.VehiclesToIgnore, lane.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true));
final.Decorators = tp.Decorators;
tp.Behavior = final;
}
else
{
SupraLane sl = (SupraLane)lane;
StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState;
Behavior final = sisls.GetBehavior(sc, state.Front, this.VehiclesToIgnore);
final.Decorators = tp.Decorators;
tp.Behavior = final;
}
}
#endregion
#region Normal Following
// else normal
else
{
// speed command
SpeedCommand sc = new ScalarSpeedCommand(fvtc.vFollowing);
tp.DistanceToGo = fvtc.xDistanceToGood;
tp.NextState = CoreCommon.CorePlanningState;
tp.RecommendedSpeed = fvtc.vFollowing;
tp.Type = TravellingType.Vehicle;
tp.UsingSpeed = true;
tp.SpeedCommand = sc;
if (lane is ArbiterLane)
{
ArbiterLane al = ((ArbiterLane)lane);
// standard path following behavior
Behavior final = new StayInLaneBehavior(al.LaneId, sc, this.VehiclesToIgnore, lane.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true));
final.Decorators = tp.Decorators;
tp.Behavior = final;
}
else
{
SupraLane sl = (SupraLane)lane;
StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState;
Behavior final = sisls.GetBehavior(sc, state.Front, this.VehiclesToIgnore);
final.Decorators = tp.Decorators;
tp.Behavior = final;
}
}
#endregion
#endregion
#region Check for Oncoming Vehicles
// check if need to add current lane oncoming vehicle decorator
if (false && this.CurrentVehicle.PassedDelayedBirth && fvtc.forwardOncoming && fvtc.xSeparation > TahoeParams.VL && fvtc.xSeparation < 30)
{
// check valid lane area
if (lane is ArbiterLane || ((SupraLane)lane).ClosestComponent(this.CurrentVehicle.ClosestPosition) == SLComponentType.Initial)
{
// get distance to and speed of the forward vehicle
double fvDistance = fvtc.xSeparation;
double fvSpeed = fvtc.vTarget;
// create the 5mph behavior
ScalarSpeedCommand updated = new ScalarSpeedCommand(2.24);
// set that we are using speed
tp.UsingSpeed = true;
tp.RecommendedSpeed = updated.Speed;
tp.DistanceToGo = fvtc.xSeparation;
// create the decorator
OncomingVehicleDecorator ovd = new OncomingVehicleDecorator(updated, fvDistance, fvSpeed);
// add the decorator
tp.Behavior.Decorators.Add(ovd);
tp.Decorators.Add(ovd);
}
}
#endregion
// set current
this.followingParameters = tp;
// return parameterization
return tp;
}
/// <summary>
/// Makes new parameterization for nav
/// </summary>
/// <param name="lane"></param>
/// <param name="lanePlan"></param>
/// <param name="speed"></param>
/// <param name="distance"></param>
/// <param name="stopType"></param>
/// <returns></returns>
public TravelingParameters NavStopParameterization(IFQMPlanable lane, RoadPlan roadPlan, double speed, double distance,
ArbiterWaypoint stopWaypoint, StopType stopType, VehicleState state)
{
// get min dist
double distanceCutOff = stopType == StopType.StopLine ? CoreCommon.OperationslStopLineSearchDistance : CoreCommon.OperationalStopDistance;
#region Get Decorators
// turn direction default
ArbiterTurnDirection atd = ArbiterTurnDirection.Straight;
List<BehaviorDecorator> decorators = TurnDecorators.NoDecorators;
// check if need decorators
if (lane is ArbiterLane &&
stopWaypoint.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest) &&
roadPlan.BestPlan.laneWaypointOfInterest.IsExit &&
distance < 40.0)
{
if (roadPlan.BestPlan.laneWaypointOfInterest.BestExit == null)
ArbiterOutput.Output("NAV BUG: lanePlan.laneWaypointOfInterest.BestExit: FQM NavStopParameterization");
else
{
switch (roadPlan.BestPlan.laneWaypointOfInterest.BestExit.TurnDirection)
{
case ArbiterTurnDirection.Left:
decorators = TurnDecorators.LeftTurnDecorator;
atd = ArbiterTurnDirection.Left;
break;
case ArbiterTurnDirection.Right:
atd = ArbiterTurnDirection.Right;
decorators = TurnDecorators.RightTurnDecorator;
break;
case ArbiterTurnDirection.Straight:
atd = ArbiterTurnDirection.Straight;
decorators = TurnDecorators.NoDecorators;
break;
case ArbiterTurnDirection.UTurn:
atd = ArbiterTurnDirection.UTurn;
decorators = TurnDecorators.LeftTurnDecorator;
break;
}
}
}
else if (lane is SupraLane)
{
SupraLane sl = (SupraLane)lane;
double distToInterconnect = sl.DistanceBetween(state.Front, sl.Interconnect.InitialGeneric.Position);
if ((distToInterconnect > 0 && distToInterconnect < 40.0) || sl.ClosestComponent(state.Front) == SLComponentType.Interconnect)
{
switch (sl.Interconnect.TurnDirection)
{
case ArbiterTurnDirection.Left:
decorators = TurnDecorators.LeftTurnDecorator;
atd = ArbiterTurnDirection.Left;
break;
case ArbiterTurnDirection.Right:
atd = ArbiterTurnDirection.Right;
decorators = TurnDecorators.RightTurnDecorator;
break;
case ArbiterTurnDirection.Straight:
atd = ArbiterTurnDirection.Straight;
decorators = TurnDecorators.NoDecorators;
break;
case ArbiterTurnDirection.UTurn:
atd = ArbiterTurnDirection.UTurn;
decorators = TurnDecorators.LeftTurnDecorator;
break;
}
}
}
#endregion
#region Get Maneuver
Maneuver m = new Maneuver();
bool usingSpeed = true;
SpeedCommand sc = new StopAtDistSpeedCommand(distance);
#region Distance Cutoff
// check if distance is less than cutoff
if (distance < distanceCutOff && stopType != StopType.EndOfLane)
{
// default behavior
Behavior b = new StayInLaneBehavior(stopWaypoint.Lane.LaneId, new StopAtDistSpeedCommand(distance), new List<int>(), lane.LanePath(), stopWaypoint.Lane.Width, stopWaypoint.Lane.NumberOfLanesLeft(state.Front, true), stopWaypoint.Lane.NumberOfLanesRight(state.Front, true));
// stopping so not using speed param
usingSpeed = false;
// exit is next
if (stopType == StopType.Exit)
{
// exit means stopping at a good exit in our current lane
IState nextState = new StoppingAtExitState(stopWaypoint.Lane, stopWaypoint, atd, true, roadPlan.BestPlan.laneWaypointOfInterest.BestExit, state.Timestamp, state.Front);
m = new Maneuver(b, nextState, decorators, state.Timestamp);
}
// stop line is left
else if (stopType == StopType.StopLine)
{
// determine if hte stop line is the best exit
bool isNavExit = roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Equals(stopWaypoint);
// get turn direction
atd = isNavExit ? atd : ArbiterTurnDirection.Straight;
// predetermine interconnect if best exit
ArbiterInterconnect desired = null;
if (isNavExit)
desired = roadPlan.BestPlan.laneWaypointOfInterest.BestExit;
else if (stopWaypoint.NextPartition != null && state.Front.DistanceTo(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position) > 25)
desired = stopWaypoint.NextPartition.ToInterconnect;
// set decorators
decorators = isNavExit ? decorators : TurnDecorators.NoDecorators;
// stop at the stop
IState nextState = new StoppingAtStopState(stopWaypoint.Lane, stopWaypoint, atd, isNavExit, desired);
b = new StayInLaneBehavior(stopWaypoint.Lane.LaneId, new StopAtLineSpeedCommand(), new List<int>(), lane.LanePath(), stopWaypoint.Lane.Width, stopWaypoint.Lane.NumberOfLanesLeft(state.Front, true), stopWaypoint.Lane.NumberOfLanesRight(state.Front, true));
m = new Maneuver(b, nextState, decorators, state.Timestamp);
sc = new StopAtLineSpeedCommand();
}
else if(stopType == StopType.LastGoal)
{
// stop at the last goal
IState nextState = new StayInLaneState(stopWaypoint.Lane, CoreCommon.CorePlanningState);
m = new Maneuver(b, nextState, decorators, state.Timestamp);
}
}
#endregion
#region Outisde Distance Envelope
// not inside distance envalope
else
{
// set speed
sc = new ScalarSpeedCommand(speed);
// check if lane
if (lane is ArbiterLane)
{
// get lane
ArbiterLane al = (ArbiterLane)lane;
// default behavior
Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(speed), new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true));
// standard behavior is fine for maneuver
m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), decorators, state.Timestamp);
}
// check if supra lane
else if (lane is SupraLane)
{
// get lane
SupraLane sl = (SupraLane)lane;
// get sl state
StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState;
// get default beheavior
Behavior b = sisls.GetBehavior(new ScalarSpeedCommand(speed), state.Front, new List<int>());
// standard behavior is fine for maneuver
m = new Maneuver(b, sisls, decorators, state.Timestamp);
}
}
#endregion
#endregion
#region Parameterize
// create new params
TravelingParameters tp = new TravelingParameters();
tp.Behavior = m.PrimaryBehavior;
tp.Decorators = m.PrimaryBehavior.Decorators;
tp.DistanceToGo = distance;
tp.NextState = m.PrimaryState;
tp.RecommendedSpeed = speed;
tp.Type = TravellingType.Navigation;
tp.UsingSpeed = usingSpeed;
tp.SpeedCommand = sc;
tp.VehiclesToIgnore = new List<int>();
// return navigation params
return tp;
#endregion
}
/// <summary>
/// Resume from pause
/// </summary>
/// <returns></returns>
public Behavior Resume(VehicleState currentState, double speed)
{
// new default stay in lane behavior
Behavior b = new StayInLaneBehavior(Lane.LaneId, new ScalarSpeedCommand(0.0), new int[] { }, Lane.LanePath(), Lane.Width, Lane.NumberOfLanesLeft(currentState.Position, true), Lane.NumberOfLanesRight(currentState.Position, true));
// set default blinkers
b.Decorators = this.DefaultStateDecorators;
// return behavior
return b;
}
private void HandleBehavior(StayInLaneBehavior b)
{
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(b.TimeStamp);
this.behaviorTimestamp = absTransform.Timestamp;
this.rndfPath = b.BackupPath.Transform(absTransform);
this.rndfPathWidth = b.LaneWidth;
this.numLanesLeft = b.NumLaneLeft;
this.numLanesRight = b.NumLanesRight;
this.laneID = b.TargetLane;
this.ignorableObstacles = b.IgnorableObstacles;
Services.ObstaclePipeline.LastIgnoredObstacles = b.IgnorableObstacles;
HandleSpeedCommand(b.SpeedCommand);
opposingLaneVehicleExists = false;
oncomingVehicleExists = false;
extraWidth = 0;
if (b.Decorators != null) {
foreach (BehaviorDecorator d in b.Decorators) {
if (d is OpposingLaneDecorator) {
opposingLaneVehicleExists = true;
opposingLaneVehicleDist = ((OpposingLaneDecorator)d).Distance;
opposingLaneVehicleSpeed = ((OpposingLaneDecorator)d).Speed;
}
else if (d is OncomingVehicleDecorator) {
oncomingVehicleExists = true;
oncomingVehicleDist = ((OncomingVehicleDecorator)d).TargetDistance;
oncomingVehicleSpeed = ((OncomingVehicleDecorator)d).TargetSpeed;
oncomingVehicleSpeedCommand = ((OncomingVehicleDecorator)d).SecondarySpeed;
}
else if (d is WidenBoundariesDecorator) {
extraWidth = ((WidenBoundariesDecorator)d).ExtraWidth;
}
}
}
if (oncomingVehicleExists) {
double timeToCollision = oncomingVehicleDist/Math.Abs(oncomingVehicleSpeed);
if (oncomingVehicleDist > 30 || timeToCollision > 20 || numLanesRight > 0) {
oncomingVehicleExists = false;
}
else {
HandleSpeedCommand(oncomingVehicleSpeedCommand);
}
}
if (laneID != null && Services.RoadNetwork != null) {
ArbiterLane lane = Services.RoadNetwork.ArbiterSegments[laneID.SegmentId].Lanes[laneID];
AbsolutePose pose = Services.StateProvider.GetAbsolutePose();
sparse = (lane.GetClosestPartition(pose.xy).Type == PartitionType.Sparse);
if (sparse){
LinePath.PointOnPath closest = b.BackupPath.GetClosestPoint(pose.xy);
goalPoint = b.BackupPath[closest.Index+1];
}
}
Services.UIService.PushAbsolutePath(b.BackupPath, b.TimeStamp, "original path1");
Services.UIService.PushAbsolutePath(new LineList(), b.TimeStamp, "original path2");
if (sparse) {
if (Services.ObstaclePipeline.ExtraSpacing == 0) {
Services.ObstaclePipeline.ExtraSpacing = 0.5;
}
Services.ObstaclePipeline.UseOccupancyGrid = false;
}
else {
Services.ObstaclePipeline.ExtraSpacing = 0;
Services.ObstaclePipeline.UseOccupancyGrid = true;
smootherSpacingAdjust = 0;
prevCurvature = double.NaN;
}
}
/// <summary>
/// Secondary maneuver when current lane is the desired lane
/// </summary>
/// <param name="arbiterLane"></param>
/// <param name="vehicleState"></param>
/// <param name="roadPlan"></param>
/// <param name="blockages"></param>
/// <param name="ignorable"></param>
/// <returns></returns>
public Maneuver? AdvancedSecondaryNextCheck(ArbiterLane lane, ArbiterWaypoint laneGoal, VehicleState vehicleState, RoadPlan roadPlan,
List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, TypeOfTasks bestTask)
{
// check if the forward vehicle exists
if (this.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker && this.ForwardMonitor.ForwardVehicle.CurrentVehicle.PassedLongDelayedBirth)
{
// distance to forward vehicle
double distanceToForwardVehicle = lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);
#region Distance From Forward Not Enough
// check distance to forward vehicle
if (this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.Queuing == QueuingState.Failed &&
distanceToForwardVehicle < 8.0 && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle)
{
// set name
ArbiterLane al = lane;
// distance to forward vehicle too small
double distToForwards = al.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);
double distToReverse = Math.Max(1.0, 8.0 - distToForwards);
if (distToForwards < 8.0)
{
// notify
ArbiterOutput.Output("Secondary: NOT Properly Stopped Behind Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " distance: " + distToForwards.ToString("f2"));
this.RearMonitor = new RearQuadrantMonitor(al, SideObstacleSide.Driver);
this.RearMonitor.Update(vehicleState);
if (this.RearMonitor.CurrentVehicle != null)
{
double distToRearVehicle = al.DistanceBetween(this.RearMonitor.CurrentVehicle.ClosestPosition, vehicleState.Position) - TahoeParams.RL;
double distNeedClear = distToReverse + 2.0;
if (distToRearVehicle < distNeedClear)
{
// notify
ArbiterOutput.Output("Secondary: Rear: Not enough room to clear in rear: " + distToRearVehicle.ToString("f2") + " < " + distNeedClear.ToString("f2"));
return null;
}
}
double distToLaneStart = al.DistanceBetween(al.LanePath().StartPoint.Location, vehicleState.Position) - TahoeParams.RL;
if (distToReverse > distToLaneStart)
{
// notify
ArbiterOutput.Output("Secondary: Rear: Not enough room in lane to reverse in rear: " + distToLaneStart.ToString("f2") + " < " + distToReverse.ToString("f2"));
return null;
}
else
{
// notify
ArbiterOutput.Output("Secondary: Reversing to pass Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " reversing distance: " + distToReverse.ToString("f2"));
StopAtDistSpeedCommand sadsc = new StopAtDistSpeedCommand(distToReverse, true);
StayInLaneBehavior silb = new StayInLaneBehavior(al.LaneId, sadsc, new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(vehicleState.Front, true), al.NumberOfLanesRight(vehicleState.Front, true));
return new Maneuver(silb, CoreCommon.CorePlanningState, TurnDecorators.HazardDecorator, vehicleState.Timestamp);
}
}
}
#endregion
// get distance to next lane major
List<WaypointType> wts = new List<WaypointType>(new WaypointType[] { WaypointType.Stop, WaypointType.End });
ArbiterWaypoint nextWaypoint = lane.GetNext(lane.GetClosestPartition(vehicleState.Position).Final, wts, new List<ArbiterWaypoint>());
// check if the vehicle occurs before the next major thing
double distanceToNextMajor = lane.DistanceBetween(vehicleState.Front, nextWaypoint.Position);
// check if vehicle occurs before the next lane major
if (distanceToForwardVehicle < distanceToNextMajor)
{
// check if forward vehicle exists in this lane and is closer then the lane goal
if (TacticalDirector.VehicleAreas.ContainsKey(lane) &&
TacticalDirector.VehicleAreas[lane].Contains(this.ForwardMonitor.ForwardVehicle.CurrentVehicle) &&
lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) < lane.DistanceBetween(vehicleState.Front, laneGoal.Position))
{
// some constants
double desiredDistance = 50.0;
// get the lane goal distance
double distanceToLaneGoal = lane.DistanceBetween(vehicleState.Front, laneGoal.Position);
// check if necessary to be in current lane
if (distanceToLaneGoal <= desiredDistance)
{
// default secondary maneuver
ArbiterOutput.WriteToLog("AdvancedSecondaryNextCheck: DistToLaneGoal < 50: " + desiredDistance.ToString("f2"));
return this.SecondaryManeuver(lane, vehicleState, roadPlan, new List<ITacticalBlockage>(), new List<ArbiterWaypoint>(), bestTask);
}
// otherwise would be good but not necessary
else
{
// return the maneuver from the vehicle being outside the min cap
ArbiterOutput.WriteToLog("AdvancedSecondaryNextCheck: DistToLaneGoal > 50: " + desiredDistance.ToString("f2"));
return this.AdvancedSecondaryOutsideMinCap(lane, laneGoal, vehicleState, roadPlan, blockages, ignorable, bestTask);
}
}
}
}
// no secondary
return null;
}
/// <summary>
/// Makes use of parameterizations made from the initial forward maneuver plan
/// </summary>
/// <param name="arbiterLane"></param>
/// <param name="vehicleState"></param>
/// <param name="roadPlan"></param>
/// <param name="blockages"></param>
/// <param name="ignorable"></param>
/// <returns></returns>
public Maneuver? SecondaryManeuver(IFQMPlanable arbiterLane, VehicleState vehicleState, RoadPlan roadPlan,
List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, TypeOfTasks bestTask)
{
// check if we might be able to pass here
bool validArea = arbiterLane is ArbiterLane || (((SupraLane)arbiterLane).ClosestComponent(vehicleState.Front) == SLComponentType.Initial);
ArbiterLane ourForwardLane = arbiterLane is ArbiterLane ? (ArbiterLane)arbiterLane : ((SupraLane)arbiterLane).Initial;
// check if the forward vehicle exists and we're in a valid area
if (this.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker && validArea)
{
// check if we should pass the vehicle ahead
LaneChangeInformation lci;
bool sp = this.ForwardMonitor.ForwardVehicle.ShouldPass(out lci);
// make sure we should do something before processing extras
if(sp)
{
// available parameterizations for the lane change
List<LaneChangeParameters> changeParams = new List<LaneChangeParameters>();
// get lane
ArbiterLane al = arbiterLane is ArbiterLane ? (ArbiterLane)arbiterLane : ((SupraLane)arbiterLane).Initial;
// get the location we need to return by
Coordinates absoluteUpperBound = arbiterLane is ArbiterLane ?
roadPlan.LanePlans[al.LaneId].laneWaypointOfInterest.PointOfInterest.Position :
((SupraLane)arbiterLane).Interconnect.InitialGeneric.Position;
#region Failed Forward
// if failed, parameterize ourselved if we're following them
if (lci.Reason == LaneChangeReason.FailedForwardVehicle && this.ForwardMonitor.CurrentParameters.Type == TravellingType.Vehicle)
{
// notify
ArbiterOutput.Output("Failed Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.VehicleId.ToString());
// get traveling params from FQM to make sure we stopped for vehicle, behind vehicle
double v = CoreCommon.Communications.GetVehicleSpeed().Value;
TravelingParameters fqmParams = this.ForwardMonitor.CurrentParameters;
double d = this.ForwardMonitor.ForwardVehicle.DistanceToVehicle(arbiterLane, vehicleState.Front);
Coordinates departUpperBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d - 3.0).Location;
// check stopped behing failed forward
try
{
if (fqmParams.Type == TravellingType.Vehicle && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle)
{
// check for checkpoint within 4VL of front of failed vehicle
ArbiterCheckpoint acCurrecnt = CoreCommon.Mission.MissionCheckpoints.Peek();
if (acCurrecnt.WaypointId.AreaSubtypeId.Equals(al.LaneId))
{
// check distance
ArbiterWaypoint awCheckpoint = (ArbiterWaypoint)CoreCommon.RoadNetwork.ArbiterWaypoints[acCurrecnt.WaypointId];
double cpDistacne = Lane.DistanceBetween(vehicleState.Front, awCheckpoint.Position);
if (cpDistacne < d || cpDistacne - d < TahoeParams.VL * 4.5)
{
ArbiterOutput.Output("Removing checkpoint: " + acCurrecnt.WaypointId.ToString() + " as failed vehicle over it");
CoreCommon.Mission.MissionCheckpoints.Dequeue();
return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
}
}catch (Exception) { }
#region Right Lateral Reasoning Forwards
// check right lateral reasoning for existence, if so parametrize
if (rightLateralReasoning.Exists && fqmParams.Type == TravellingType.Vehicle && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle)
{
// get lane
ArbiterLane lane = al;
// determine failed vehicle lane change distance params
Coordinates defaultReturnLowerBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 2.0)).Location;
Coordinates minimumReturnComplete = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 3.0)).Location;
Coordinates defaultReturnUpperBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 5.0)).Location;
// get params for lane change
LaneChangeParameters? lcp = this.LaneChangeParameterization(
new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, null),
lane, lane.LaneOnRight, false, roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound, blockages, ignorable,
vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value);
// check if exists to generate full param
if (lcp.HasValue)
{
// get param
LaneChangeParameters tp = lcp.Value;
// notify
ArbiterOutput.WriteToLog("Failed Forward: Right Lateral Reasoning Forwards: Available: " + tp.Available.ToString() + ", Feasable: " + tp.Feasible.ToString());
// get behavior
ChangeLaneBehavior clb = new ChangeLaneBehavior(
al.LaneId, al.LaneOnRight.LaneId, false, al.DistanceBetween(vehicleState.Front, departUpperBound),
new ScalarSpeedCommand(tp.Parameters.RecommendedSpeed), tp.Parameters.VehiclesToIgnore,
al.LanePath(), al.LaneOnRight.LanePath(), al.Width, al.LaneOnRight.Width, al.NumberOfLanesLeft(vehicleState.Front, true), al.NumberOfLanesRight(vehicleState.Front, true));
tp.Behavior = clb;
tp.Decorators = TurnDecorators.RightTurnDecorator;
// next state
ChangeLanesState cls = new ChangeLanesState(tp);
tp.NextState = cls;
// add parameterization to possible
changeParams.Add(tp);
}
}
#endregion
#region Left Lateral Reasoning
// check left lateral reasoning
if(leftLateralReasoning.Exists)
{
#region Left Lateral Opposing
// check opposing
ArbiterLane closestOpposingLane = this.GetClosestOpposing(ourForwardLane, vehicleState);
if(closestOpposingLane != null && (leftLateralReasoning.IsOpposing || !closestOpposingLane.Equals(leftLateralReasoning.LateralLane)))
{
// check room of initial
bool enoughRoom =
(arbiterLane is ArbiterLane && (!roadPlan.BestPlan.laneWaypointOfInterest.IsExit || ((ArbiterLane)arbiterLane).DistanceBetween(vehicleState.Front, roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position) > TahoeParams.VL * 5.0)) ||
(arbiterLane is SupraLane && ((SupraLane)arbiterLane).DistanceBetween(vehicleState.Front, ((SupraLane)arbiterLane).Interconnect.InitialGeneric.Position) > TahoeParams.VL * 5.0);
// check opposing enough room
bool oppEnough = closestOpposingLane.DistanceBetween(closestOpposingLane.LanePath().StartPoint.Location, vehicleState.Front) > TahoeParams.VL * 5.0;
// check if enough room
if (enoughRoom && oppEnough)
{
// check if we're stopped and the current trav params were for a vehicle and we're close to the vehicle
bool stoppedBehindFV = fqmParams.Type == TravellingType.Vehicle && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle;
// check that we're stopped behind forward vehicle before attempting to change lanes
if (stoppedBehindFV)
{
#region Check Segment Blockage
// check need to make uturn (hack)
bool waitForUTurnCooldown;
BlockageTactical bt = CoreCommon.BlockageDirector;
StayInLaneBehavior tmpBlockBehavior = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(2.0), new List<int>(), al.LanePath(), al.Width, 0, 0);
ITacticalBlockage itbTmp = new LaneBlockage(new TrajectoryBlockedReport(CompletionResult.Stopped, TahoeParams.VL, BlockageType.Static, -1, false, tmpBlockBehavior.GetType()));
Maneuver tmpBlockManeuver = bt.LaneRecoveryManeuver(al, vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value, roadPlan,
new BlockageRecoveryState(tmpBlockBehavior,
new StayInLaneState(al, CoreCommon.CorePlanningState), new StayInLaneState(al, CoreCommon.CorePlanningState), BlockageRecoveryDEFCON.REVERSE,
new EncounteredBlockageState(itbTmp, CoreCommon.CorePlanningState, BlockageRecoveryDEFCON.REVERSE, SAUDILevel.None), BlockageRecoverySTATUS.ENCOUNTERED), true, out waitForUTurnCooldown);
if (!waitForUTurnCooldown && tmpBlockManeuver.PrimaryBehavior is UTurnBehavior)
return tmpBlockManeuver;
else if (waitForUTurnCooldown)
return null;
#endregion
// distance to forward vehicle too small
double distToForwards = al.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);
double distToReverse = Math.Max(1.0, 8.0 - distToForwards);
if (distToForwards < 8.0)
{
// notify
ArbiterOutput.WriteToLog("Secondary: NOT Properly Stopped Behind Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " distance: " + distToForwards.ToString("f2"));
this.RearMonitor = new RearQuadrantMonitor(al, SideObstacleSide.Driver);
this.RearMonitor.Update(vehicleState);
if (this.RearMonitor.CurrentVehicle != null)
{
double distToRearVehicle = al.DistanceBetween(this.RearMonitor.CurrentVehicle.ClosestPosition, vehicleState.Position) - TahoeParams.RL;
double distNeedClear = distToReverse + 2.0;
if (distToRearVehicle < distNeedClear)
{
// notify
ArbiterOutput.Output("Secondary: Rear: Not enough room to clear in rear: " + distToRearVehicle.ToString("f2") + " < " + distNeedClear.ToString("f2"));
return null;
}
}
double distToLaneStart = al.DistanceBetween(al.LanePath().StartPoint.Location, vehicleState.Position) - TahoeParams.RL;
if (distToReverse > distToLaneStart)
{
// notify
ArbiterOutput.Output("Secondary: Rear: Not enough room in lane to reverse in rear: " + distToLaneStart.ToString("f2") + " < " + distToReverse.ToString("f2"));
return null;
}
else
{
// notify
ArbiterOutput.Output("Secondary: Reversing to pass Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " reversing distance: " + distToReverse.ToString("f2"));
StopAtDistSpeedCommand sadsc = new StopAtDistSpeedCommand(distToReverse, true);
StayInLaneBehavior silb = new StayInLaneBehavior(al.LaneId, sadsc, new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(vehicleState.Front, true), al.NumberOfLanesRight(vehicleState.Front, true));
return new Maneuver(silb, CoreCommon.CorePlanningState, TurnDecorators.HazardDecorator, vehicleState.Timestamp);
}
}
else
{
// notify
ArbiterOutput.WriteToLog("Secondary: Left Lateral Opposing: Properly Stopped Behind Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString());
// determine failed vehicle lane change distance params
Coordinates defaultReturnLowerBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 2.0)).Location;
Coordinates minimumReturnComplete = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 3.5)).Location;
Coordinates defaultReturnUpperBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 5.0)).Location;
// check if enough room
if (al.DistanceBetween(vehicleState.Front, defaultReturnUpperBound) >= d + TahoeParams.VL * 4.5)
{
// get hte closest oppoing
ArbiterLane closestOpposing = this.GetClosestOpposing(al, vehicleState);
// check exists
if (closestOpposing != null)
{
// set/check secondary
if (this.secondaryLeftLateralReasoning == null || !this.secondaryLeftLateralReasoning.LateralLane.Equals(closestOpposing))
this.secondaryLeftLateralReasoning = new OpposingLateralReasoning(closestOpposing, SideObstacleSide.Driver);
// check the state of hte lanes next to us
if (this.leftLateralReasoning.LateralLane.Equals(closestOpposing) && this.leftLateralReasoning.ExistsExactlyHere(vehicleState))
{
#region Plan
// need to make sure that we wait for 3 seconds with the blinker on (resetting with pause)
if (this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.ElapsedMilliseconds > 3000)
{
// notify
ArbiterOutput.Output("Scondary: Left Lateral Opposing: Wait Timer DONE");
// get parameterization
LaneChangeParameters? tp = this.LaneChangeParameterization(
new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, this.ForwardMonitor.ForwardVehicle.CurrentVehicle),
al,
leftLateralReasoning.LateralLane,
true,
roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
departUpperBound,
defaultReturnLowerBound,
minimumReturnComplete,
defaultReturnUpperBound,
blockages,
ignorable,
vehicleState,
CoreCommon.Communications.GetVehicleSpeed().Value);
// check if available and feasible
if (tp.HasValue && tp.Value.Available && tp.Value.Feasible)
{
// notify
ArbiterOutput.Output("Scondary: Left Lateral Opposing: AVAILABLE & FEASIBLE");
LaneChangeParameters lcp = tp.Value;
lcp.Behavior = this.ForwardMonitor.CurrentParameters.Behavior;
lcp.Decorators = TurnDecorators.LeftTurnDecorator;
lcp.Behavior.Decorators = lcp.Decorators;
// next state
ChangeLanesState cls = new ChangeLanesState(tp.Value);
lcp.NextState = cls;
// add parameterization to possible
changeParams.Add(lcp);
}
// check if not available now but still feasible
else if (tp.HasValue && !tp.Value.Available && tp.Value.Feasible)
{
// notify
ArbiterOutput.Output("Scondary: Left Lateral Opposing: NOT Available, Still FEASIBLE, WAITING");
// wait and blink maneuver
TravelingParameters tp2 = this.ForwardMonitor.CurrentParameters;
tp2.Decorators = TurnDecorators.LeftTurnDecorator;
tp2.Behavior.Decorators = tp2.Decorators;
// create parameterization
LaneChangeParameters lcp = new LaneChangeParameters(false, true, al, false, al.LaneOnLeft,
true, true, tp2.Behavior, 0.0, CoreCommon.CorePlanningState, tp2.Decorators, tp2, new Coordinates(),
new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.FailedForwardVehicle);
// add parameterization to possible
changeParams.Add(lcp);
}
}
// otherwise timer not running or not been long enough
else
{
// check if timer running
if (!this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.IsRunning)
this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Start();
double waited = (double)(this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.ElapsedMilliseconds / 1000.0);
ArbiterOutput.Output("Waited for failed forwards: " + waited.ToString("F2") + " seconds");
// wait and blink maneuver
TravelingParameters tp = this.ForwardMonitor.CurrentParameters;
tp.Decorators = TurnDecorators.LeftTurnDecorator;
tp.Behavior.Decorators = tp.Decorators;
// create parameterization
LaneChangeParameters lcp = new LaneChangeParameters(false, true, al, false, al.LaneOnLeft,
true, true, tp.Behavior, 0.0, CoreCommon.CorePlanningState, tp.Decorators, tp, new Coordinates(),
new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.FailedForwardVehicle);
// add parameterization to possible
changeParams.Add(lcp);
}
#endregion
}
else if (!this.leftLateralReasoning.LateralLane.Equals(closestOpposing) && !this.leftLateralReasoning.ExistsRelativelyHere(vehicleState))
{
// set and notify
ArbiterOutput.Output("superceeded left lateral reasoning with override for non adjacent left lateral reasoning");
ILateralReasoning tmpReasoning = this.leftLateralReasoning;
this.leftLateralReasoning = this.secondaryLeftLateralReasoning;
try
{
#region Plan
// need to make sure that we wait for 3 seconds with the blinker on (resetting with pause)
if (this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.ElapsedMilliseconds > 3000)
{
// notify
ArbiterOutput.Output("Scondary: Left Lateral Opposing: Wait Timer DONE");
// get parameterization
LaneChangeParameters? tp = this.LaneChangeParameterization(
new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, this.ForwardMonitor.ForwardVehicle.CurrentVehicle),
al,
leftLateralReasoning.LateralLane,
true,
roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
departUpperBound,
defaultReturnLowerBound,
minimumReturnComplete,
defaultReturnUpperBound,
blockages,
ignorable,
vehicleState,
CoreCommon.Communications.GetVehicleSpeed().Value);
// check if available and feasible
if (tp.HasValue && tp.Value.Available && tp.Value.Feasible)
{
// notify
ArbiterOutput.Output("Scondary: Left Lateral Opposing: AVAILABLE & FEASIBLE");
LaneChangeParameters lcp = tp.Value;
lcp.Behavior = this.ForwardMonitor.CurrentParameters.Behavior;
lcp.Decorators = TurnDecorators.LeftTurnDecorator;
lcp.Behavior.Decorators = TurnDecorators.LeftTurnDecorator;
// next state
ChangeLanesState cls = new ChangeLanesState(tp.Value);
lcp.NextState = cls;
// add parameterization to possible
changeParams.Add(lcp);
}
// check if not available now but still feasible
else if (tp.HasValue && !tp.Value.Available && tp.Value.Feasible)
{
// notify
ArbiterOutput.Output("Scondary: Left Lateral Opposing: NOT Available, Still FEASIBLE, WAITING");
// wait and blink maneuver
TravelingParameters tp2 = this.ForwardMonitor.CurrentParameters;
tp2.Decorators = TurnDecorators.LeftTurnDecorator;
tp2.Behavior.Decorators = tp2.Decorators;
// create parameterization
LaneChangeParameters lcp = new LaneChangeParameters(false, true, al, false, this.leftLateralReasoning.LateralLane,
true, true, tp2.Behavior, 0.0, CoreCommon.CorePlanningState, tp2.Decorators, tp2, new Coordinates(),
new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.FailedForwardVehicle);
// add parameterization to possible
changeParams.Add(lcp);
}
}
// otherwise timer not running or not been long enough
else
{
// check if timer running
if (!this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.IsRunning)
this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Start();
double waited = (double)(this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.ElapsedMilliseconds / 1000.0);
ArbiterOutput.Output("Waited for failed forwards: " + waited.ToString("F2") + " seconds");
// wait and blink maneuver
TravelingParameters tp = this.ForwardMonitor.CurrentParameters;
tp.Decorators = TurnDecorators.LeftTurnDecorator;
tp.Behavior.Decorators = tp.Decorators;
// create parameterization
LaneChangeParameters lcp = new LaneChangeParameters(false, true, al, false, this.leftLateralReasoning.LateralLane,
true, true, tp.Behavior, 0.0, CoreCommon.CorePlanningState, tp.Decorators, tp, new Coordinates(),
new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.FailedForwardVehicle);
// add parameterization to possible
changeParams.Add(lcp);
}
#endregion
}
catch (Exception ex)
{
ArbiterOutput.Output("Core intelligence thread caught exception in forward reasoning secondary maneuver when non-standard adjacent left: " + ex.ToString());
}
// restore
this.leftLateralReasoning = tmpReasoning;
}
}
else
{
// do nuttin
ArbiterOutput.Output("no opposing adjacent");
}
}
else
{
// notify
ArbiterOutput.Output("Secondary: LeftLatOpp: Stopped Behind FV: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + ", but not enough room to pass");
}
}
}
else
{
this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Stop();
this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Reset();
// notify
ArbiterOutput.Output("Secondary: Left Lateral Opposing: NOT Stopped Behind Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString());
}
}
else
{
ArbiterOutput.Output("Secondary Opposing: enough room to pass opposing: initial: " + enoughRoom.ToString() + ", opposing: " + oppEnough.ToString());
}
}
#endregion
#region Left Lateral Forwards
// otherwise parameterize
else if(fqmParams.Type == TravellingType.Vehicle && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle)
{
// get lane
ArbiterLane lane = al;
// determine failed vehicle lane change distance params
Coordinates defaultReturnLowerBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 2.0)).Location;
Coordinates minimumReturnComplete = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 3.0)).Location;
Coordinates defaultReturnUpperBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 5.0)).Location;
// get params for lane change
LaneChangeParameters? lcp = this.LaneChangeParameterization(
new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, null),
lane, lane.LaneOnLeft, false, roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound,
blockages, ignorable, vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value);
// check if exists to generate full param
if (lcp.HasValue)
{
// set param
LaneChangeParameters tp = lcp.Value;
// notify
ArbiterOutput.Output("Secondary Failed Forward Reasoning Forwards: Available: " + tp.Available.ToString() + ", Feasible: " + tp.Feasible.ToString());
// get behavior
ChangeLaneBehavior clb = new ChangeLaneBehavior(
al.LaneId, al.LaneOnLeft.LaneId, true, al.DistanceBetween(vehicleState.Front, departUpperBound),
new ScalarSpeedCommand(tp.Parameters.RecommendedSpeed), tp.Parameters.VehiclesToIgnore,
al.LanePath(), al.LaneOnLeft.LanePath(), al.Width, al.LaneOnLeft.Width, al.NumberOfLanesLeft(vehicleState.Front, true), al.NumberOfLanesRight(vehicleState.Front, true));
tp.Behavior = clb;
tp.Decorators = TurnDecorators.LeftTurnDecorator;
// next state
ChangeLanesState cls = new ChangeLanesState(tp);
tp.NextState = cls;
// add parameterization to possible
changeParams.Add(tp);
}
}
#endregion
}
#endregion
}
#endregion
#region Slow Forward
// if pass, determine if should pass in terms or vehicles adjacent and in front then call lane change function for maneuver
else if (lci.Reason == LaneChangeReason.SlowForwardVehicle)
{
// if left exists and is not opposing, parameterize
if (leftLateralReasoning.Exists && !leftLateralReasoning.IsOpposing)
{
throw new Exception("slow forward vehicle pass not implemented yet");
}
// if right exists and is not opposing, parameterize
if (rightLateralReasoning.Exists && !rightLateralReasoning.IsOpposing)
{
throw new Exception("slow forward vehicle pass not implemented yet");
}
}
#endregion
#region Parameterize
// check params to see if any are good and available
if(changeParams != null && changeParams.Count > 0)
{
// sort the parameterizations
changeParams.Sort();
// get first
LaneChangeParameters final = changeParams[0];
// notify
ArbiterOutput.Output("Secondary Reasoning Final: Available: " + final.Available.ToString() + ", Feasible: " + final.Feasible.ToString());
// make sure ok
if (final.Available && final.Feasible)
{
// return best param
return new Maneuver(changeParams[0].Behavior, changeParams[0].NextState, changeParams[0].Decorators, vehicleState.Timestamp);
}
}
#endregion
}
}
// fallout is null
return null;
}
/// <summary>
/// Parameters to follow the forward vehicle
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
/// <returns></returns>
public TravelingParameters Follow(ArbiterLane lane, VehicleState state)
{
// travelling parameters
TravelingParameters tp = new TravelingParameters();
// ignorable obstacles
List<int> ignoreVehicle = new List<int>();
ignoreVehicle.Add(CurrentVehicle.VehicleId);
// get control parameters
ForwardVehicleTrackingControl fvtc = GetControl(lane, state);
// init params
tp.DistanceToGo = fvtc.xDistanceToGood;
tp.NextState = CoreCommon.CorePlanningState;
tp.RecommendedSpeed = fvtc.vFollowing;
tp.Type = TravellingType.Vehicle;
tp.Decorators = TurnDecorators.NoDecorators;
// flag to ignore forward vehicle
bool ignoreForward = false;
// reversed lane path
LinePath lp = lane.LanePath().Clone();
lp.Reverse();
#region Following Control
#region Immediate Stop
// need to stop immediately
if (fvtc.vFollowing == 0.0)
{
// don't ignore forward
ignoreForward = false;
// speed command
SpeedCommand sc = new ScalarSpeedCommand(0.0);
tp.UsingSpeed = true;
// standard path following behavior
Behavior final = new StayInLaneBehavior(lane.LaneId, sc, ignoreVehicle, lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false));
tp.Behavior = final;
tp.SpeedCommand = sc;
}
#endregion
#region Distance Stop
// stop at distance
else if (fvtc.vFollowing < 0.7 &&
CoreCommon.Communications.GetVehicleSpeed().Value <= 2.24 &&
fvtc.xSeparation > fvtc.xAbsMin)
{
// ignore forward vehicle
ignoreForward = true;
// speed command
SpeedCommand sc = new StopAtDistSpeedCommand(fvtc.xDistanceToGood);
tp.UsingSpeed = false;
// standard path following behavior
Behavior final = new StayInLaneBehavior(lane.LaneId, sc, ignoreVehicle, lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false));
tp.Behavior = final;
tp.SpeedCommand = sc;
}
#endregion
#region Normal Following
// else normal
else
{
// ignore the forward vehicle as we are tracking properly
ignoreForward = true;
// speed command
SpeedCommand sc = new ScalarSpeedCommand(fvtc.vFollowing);
tp.DistanceToGo = fvtc.xDistanceToGood;
tp.NextState = CoreCommon.CorePlanningState;
tp.RecommendedSpeed = fvtc.vFollowing;
tp.Type = TravellingType.Vehicle;
tp.UsingSpeed = true;
// standard path following behavior
Behavior final = new StayInLaneBehavior(lane.LaneId, sc, ignoreVehicle, lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false));
tp.Behavior = final;
tp.SpeedCommand = sc;
}
#endregion
#endregion
// check ignore
if (ignoreForward)
{
List<int> ignorable = new List<int>();
ignorable.Add(this.CurrentVehicle.VehicleId);
tp.VehiclesToIgnore = ignorable;
}
else
tp.VehiclesToIgnore = new List<int>();
// return parameterization
return tp;
}
public override void Process(object param)
{
try {
Trace.CorrelationManager.StartLogicalOperation("ChangeLanes");
if (!base.BeginProcess()){
return;
}
// check if we were given a parameter
if (param != null && param is ChangeLaneBehavior) {
ChangeLaneBehavior clParam = (ChangeLaneBehavior)param;
HandleBehavior(clParam);
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "got new param -- speed {0}, dist {1}, dist timestamp {2}", clParam.SpeedCommand, clParam.MaxDist, clParam.TimeStamp);
}
// project the lane paths up to the current time
RelativeTransform relTransform = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
LinePath curStartingPath = startingLanePath.Transform(relTransform);
LinePath curEndingPath = endingLanePath.Transform(relTransform);
// get the starting and ending lane models
ILaneModel startingLaneModel, endingLaneModel;
Services.RoadModelProvider.GetLaneChangeModels(curStartingPath, startingLaneWidth, startingNumLanesLeft, startingNumLanesRight,
curEndingPath, endingLaneWidth, changeLeft, behaviorTimestamp, out startingLaneModel, out endingLaneModel);
// calculate the max speed
// TODO: make this look ahead for slowing down
settings.maxSpeed = GetMaxSpeed(endingLanePath, endingLanePath.ZeroPoint);
// get the remaining lane change distance
double remainingDist = GetRemainingLaneChangeDistance();
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "remaining distance is {0}", remainingDist);
if (cancelled) return;
// check if we're done
if (remainingDist <= 0) {
// create a new stay in lane behavior
int deltaLanes = changeLeft ? -1 : 1;
StayInLaneBehavior stayInLane = new StayInLaneBehavior(endingLaneID, speedCommand, null, endingLanePath, endingLaneWidth, startingNumLanesLeft + deltaLanes, startingNumLanesRight - deltaLanes);
stayInLane.TimeStamp = behaviorTimestamp.ts;
Services.BehaviorManager.Execute(stayInLane, null, false);
// send completion report
ForwardCompletionReport(new SuccessCompletionReport(typeof(ChangeLaneBehavior)));
return;
}
// calculate the planning distance
double planningDist = GetPlanningDistance();
if (planningDist < remainingDist + TahoeParams.VL) {
planningDist = remainingDist + TahoeParams.VL;
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "planning dist {0}", planningDist);
// create a linearization options structure
LinearizationOptions laneOpts = new LinearizationOptions();
laneOpts.Timestamp = curTimestamp;
// get the center line of the target lane starting at the distance we want to enter into it
laneOpts.StartDistance = remainingDist;
laneOpts.EndDistance = planningDist;
LinePath centerLine = endingLaneModel.LinearizeCenterLine(laneOpts);
// add the final center line as a weighting
AddTargetPath(centerLine.Clone(), default_lane_alpha_w);
// pre-pend (0,0) as our position
centerLine.Insert(0, new Coordinates(0, 0));
LinePath leftBound = null;
LinePath rightBound = null;
// figure out if the lane is to the right or left of us
laneOpts.EndDistance = planningDist;
double leftEndingStartDist, rightEndingStartDist;
GetLaneBoundStartDists(curEndingPath, endingLaneWidth, out leftEndingStartDist, out rightEndingStartDist);
if (changeLeft) {
// we're the target lane is to the left
// get the left bound of the target lane
laneOpts.StartDistance = Math.Max(leftEndingStartDist, TahoeParams.FL);
leftBound = endingLaneModel.LinearizeLeftBound(laneOpts);
}
else {
// we're changing to the right, get the right bound of the target lane
laneOpts.StartDistance = Math.Max(rightEndingStartDist, TahoeParams.FL);
rightBound = endingLaneModel.LinearizeRightBound(laneOpts);
}
// get the other bound as the starting lane up to 5m before the remaining dist
laneOpts.StartDistance = TahoeParams.FL;
laneOpts.EndDistance = Math.Max(0, remainingDist-5);
if (changeLeft) {
if (laneOpts.EndDistance > 0) {
rightBound = startingLaneModel.LinearizeRightBound(laneOpts);
}
else {
rightBound = new LinePath();
}
}
else {
if (laneOpts.EndDistance > 0) {
leftBound = startingLaneModel.LinearizeLeftBound(laneOpts);
}
else {
leftBound = new LinePath();
}
}
// append on the that bound of the target lane starting at the remaining dist
laneOpts.StartDistance = Math.Max(remainingDist, TahoeParams.FL);
laneOpts.EndDistance = planningDist;
if (changeLeft) {
rightBound.AddRange(endingLaneModel.LinearizeRightBound(laneOpts));
}
else {
leftBound.AddRange(endingLaneModel.LinearizeLeftBound(laneOpts));
}
// set up the planning
smootherBasePath = centerLine;
AddLeftBound(leftBound, !changeLeft);
AddRightBound(rightBound, changeLeft);
Services.UIService.PushLineList(centerLine, curTimestamp, "subpath", true);
Services.UIService.PushLineList(leftBound, curTimestamp, "left bound", true);
Services.UIService.PushLineList(rightBound, curTimestamp, "right bound", true);
if (cancelled) return;
// set auxillary options
settings.endingHeading = centerLine.EndSegment.UnitVector.ArcTan;
// smooth and track that stuff
SmoothAndTrack(commandLabel, true);
}
finally {
Trace.CorrelationManager.StopLogicalOperation();
}
}
/// <summary>
/// Plan a lane change
/// </summary>
/// <param name="cls"></param>
/// <param name="initialManeuver"></param>
/// <param name="targetManeuver"></param>
/// <returns></returns>
public Maneuver PlanLaneChange(ChangeLanesState cls, VehicleState vehicleState, RoadPlan roadPlan,
List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable)
{
// check blockages
if (blockages != null && blockages.Count > 0 && blockages[0] is LaneChangeBlockage)
{
// create the blockage state
EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);
// go to a blockage handling tactical
return new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// lanes of the lane change
ArbiterLane initial = cls.Parameters.Initial;
ArbiterLane target = cls.Parameters.Target;
#region Initial Forwards
if (!cls.Parameters.InitialOncoming)
{
ForwardReasoning initialReasoning = new ForwardReasoning(new LateralReasoning(null, SideObstacleSide.Driver), new LateralReasoning(null, SideObstacleSide.Driver), initial);
#region Target Forwards
if (!cls.Parameters.TargetOncoming)
{
// target reasoning
ForwardReasoning targetReasoning = new ForwardReasoning(new LateralReasoning(null, SideObstacleSide.Driver), new LateralReasoning(null, SideObstacleSide.Driver), target);
#region Navigation
if (cls.Parameters.Reason == LaneChangeReason.Navigation)
{
// parameters to follow
List<TravelingParameters> tps = new List<TravelingParameters>();
// vehicles to ignore
List<int> ignorableVehicles = new List<int>();
// params for forward lane
initialReasoning.ForwardManeuver(initial, vehicleState, roadPlan, blockages, ignorable);
TravelingParameters initialParams = initialReasoning.ForwardMonitor.ParameterizationHelper(initial, initial,
CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ?
initial.WaypointList[initial.WaypointList.Count - 1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle);
ArbiterOutput.Output("initial dist to go: " + initialParams.DistanceToGo.ToString("f3"));
if (initialParams.Type == TravellingType.Vehicle && !initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped)
{
tps.Add(initialParams);
}
else
tps.Add(initialReasoning.ForwardMonitor.NavigationParameters);
ignorableVehicles.AddRange(initialParams.VehiclesToIgnore);
// get params for the final lane
targetReasoning.ForwardManeuver(target, vehicleState, roadPlan, blockages, new List<ArbiterWaypoint>());
TravelingParameters targetParams = targetReasoning.ForwardMonitor.CurrentParameters;
tps.Add(targetParams);
ignorableVehicles.AddRange(targetParams.VehiclesToIgnore);
try
{
if (CoreCommon.Communications.GetVehicleSpeed().Value < 0.1 &&
targetParams.Type == TravellingType.Vehicle &&
targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle != null &&
targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.Queuing == QueuingState.Failed)
{
return new Maneuver(new HoldBrakeBehavior(), new StayInLaneState(target, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
catch (Exception) { }
ArbiterOutput.Output("target dist to go: " + targetParams.DistanceToGo.ToString("f3"));
// decorators
List<BehaviorDecorator> decorators = initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target) ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator;
// distance
double distanceToGo = initial.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound);
cls.Parameters.DistanceToDepartUpperBound = distanceToGo;
// check if need to modify distance to go
if (initialParams.Type == TravellingType.Vehicle && initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped)
{
double curDistToUpper = cls.Parameters.DistanceToDepartUpperBound;
double newVhcDistToUpper = initial.DistanceBetween(vehicleState.Front, initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) - 2.0;
if (curDistToUpper > newVhcDistToUpper)
{
distanceToGo = newVhcDistToUpper;
}
}
// get final
tps.Sort();
// get the proper speed command
ScalarSpeedCommand sc = new ScalarSpeedCommand(tps[0].RecommendedSpeed);
if (sc.Speed < 8.84)
sc = new ScalarSpeedCommand(Math.Min(targetParams.RecommendedSpeed, 8.84));
// continue the lane change with the proper speed command
ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target), distanceToGo,
sc, targetParams.VehiclesToIgnore, initial.LanePath(), target.LanePath(), initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, true),
initial.NumberOfLanesRight(vehicleState.Front, true));
// standard maneuver
return new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp);
}
#endregion
#region Failed Forwards
else if (cls.Parameters.Reason == LaneChangeReason.FailedForwardVehicle)
{
// parameters to follow
List<TravelingParameters> tps = new List<TravelingParameters>();
// vehicles to ignore
List<int> ignorableVehicles = new List<int>();
// params for forward lane
initialReasoning.ForwardManeuver(initial, vehicleState, roadPlan, blockages, ignorable);
TravelingParameters initialParams = initialReasoning.ForwardMonitor.ParameterizationHelper(initial, initial,
CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ?
initial.WaypointList[initial.WaypointList.Count - 1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, null);
tps.Add(initialParams);
ignorableVehicles.AddRange(initialParams.VehiclesToIgnore);
// get params for the final lane
targetReasoning.ForwardManeuver(target, vehicleState, roadPlan, blockages, new List<ArbiterWaypoint>());
TravelingParameters targetParams = targetReasoning.ForwardMonitor.CurrentParameters;
tps.Add(targetParams);
ignorableVehicles.AddRange(targetParams.VehiclesToIgnore);
// decorators
List<BehaviorDecorator> decorators = initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target) ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator;
// distance
double distanceToGo = initial.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound);
cls.Parameters.DistanceToDepartUpperBound = distanceToGo;
// get final
tps.Sort();
// get the proper speed command
SpeedCommand sc = new ScalarSpeedCommand(tps[0].RecommendedSpeed);
// continue the lane change with the proper speed command
ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target), distanceToGo,
sc, targetParams.VehiclesToIgnore, initial.LanePath(), target.LanePath(), initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, true),
initial.NumberOfLanesRight(vehicleState.Front, true));
// standard maneuver
return new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp);
}
#endregion
#region Slow
else if (cls.Parameters.Reason == LaneChangeReason.SlowForwardVehicle)
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
#endregion
else
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
}
#endregion
#region Target Oncoming
else
{
OpposingReasoning targetReasoning = new OpposingReasoning(new OpposingLateralReasoning(null, SideObstacleSide.Driver), new OpposingLateralReasoning(null, SideObstacleSide.Driver), target);
#region Failed Forward
if (cls.Parameters.Reason == LaneChangeReason.FailedForwardVehicle)
{
// parameters to follow
List<TravelingParameters> tps = new List<TravelingParameters>();
// ignore the forward vehicle but keep params for forward lane
initialReasoning.ForwardManeuver(initial, vehicleState, roadPlan, blockages, ignorable);
TravelingParameters initialParams = initialReasoning.ForwardMonitor.ParameterizationHelper(initial, initial,
CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ?
initial.WaypointList[initial.WaypointList.Count-1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, null);
tps.Add(initialParams);
// get params for the final lane
targetReasoning.ForwardManeuver(target, initial, vehicleState, roadPlan, blockages);
TravelingParameters targetParams = targetReasoning.OpposingForwardMonitor.CurrentParamters.Value;
tps.Add(targetParams);
// decorators
List<BehaviorDecorator> decorators = cls.Parameters.ToLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator;
// distance
double distanceToGo = initial.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound);
cls.Parameters.DistanceToDepartUpperBound = distanceToGo;
// get final
tps.Sort();
// get the proper speed command
SpeedCommand sc = new ScalarSpeedCommand(Math.Min(tps[0].RecommendedSpeed, 2.24));
// check final for stopped failed opposing
VehicleAgent forwardVa = targetReasoning.OpposingForwardMonitor.ForwardVehicle.CurrentVehicle;
if (forwardVa != null)
{
// dist between
double distToFV = -targetReasoning.Lane.DistanceBetween(vehicleState.Front, forwardVa.ClosestPosition);
// check stopped
bool stopped = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.5;
// check distance
bool distOk = distToFV < 2.5 * TahoeParams.VL;
// check failed
bool failed = forwardVa.QueuingState.Queuing == QueuingState.Failed;
// notify
ArbiterOutput.Output("Forward Vehicle: Stopped: " + stopped.ToString() + ", DistOk: " + distOk.ToString() + ", Failed: " + failed.ToString());
// check all for failed
if (stopped && distOk && failed)
{
// check inside target
if (target.LanePolygon.IsInside(vehicleState.Front))
{
// blockage recovery
StayInLaneState sils = new StayInLaneState(initial, CoreCommon.CorePlanningState);
StayInLaneBehavior silb = new StayInLaneBehavior(initial.LaneId, new StopAtDistSpeedCommand(TahoeParams.VL * 2.0, true), new List<int>(), initial.LanePath(), initial.Width, initial.NumberOfLanesLeft(vehicleState.Front, false), initial.NumberOfLanesRight(vehicleState.Front, false));
BlockageRecoveryState brs = new BlockageRecoveryState(silb, sils, sils, BlockageRecoveryDEFCON.REVERSE,
new EncounteredBlockageState(new LaneBlockage(new TrajectoryBlockedReport(CompletionResult.Stopped, 4.0, BlockageType.Static, -1, true, silb.GetType())), sils, BlockageRecoveryDEFCON.INITIAL, SAUDILevel.None),
BlockageRecoverySTATUS.EXECUTING);
return new Maneuver(silb, brs, TurnDecorators.HazardDecorator, vehicleState.Timestamp);
}
// check which lane we are in
else
{
// return to forward lane
return new Maneuver(new HoldBrakeBehavior(), new StayInLaneState(initial, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
}
// continue the lane change with the proper speed command
ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, cls.Parameters.ToLeft, distanceToGo,
sc, targetParams.VehiclesToIgnore, initial.LanePath(), target.ReversePath, initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, true),
initial.NumberOfLanesRight(vehicleState.Front, true));
// standard maneuver
return new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp);
}
#endregion
#region Other
else if (cls.Parameters.Reason == LaneChangeReason.Navigation)
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
else if (cls.Parameters.Reason == LaneChangeReason.SlowForwardVehicle)
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
else
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
#endregion
}
#endregion
}
#endregion
#region Initial Oncoming
else
{
OpposingReasoning initialReasoning = new OpposingReasoning(new OpposingLateralReasoning(null, SideObstacleSide.Driver), new OpposingLateralReasoning(null, SideObstacleSide.Driver), initial);
#region Target Forwards
if (!cls.Parameters.TargetOncoming)
{
ForwardReasoning targetReasoning = new ForwardReasoning(new LateralReasoning(null, SideObstacleSide.Driver), new LateralReasoning(null, SideObstacleSide.Driver), target);
if (cls.Parameters.Reason == LaneChangeReason.FailedForwardVehicle)
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
#region Navigation
else if (cls.Parameters.Reason == LaneChangeReason.Navigation)
{
// parameters to follow
List<TravelingParameters> tps = new List<TravelingParameters>();
// distance to the upper bound of the change
double distanceToGo = target.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound);
cls.Parameters.DistanceToDepartUpperBound = distanceToGo;
// get params for the initial lane
initialReasoning.ForwardManeuver(initial, target, vehicleState, roadPlan, blockages);
// current params of the fqm
TravelingParameters initialParams = initialReasoning.OpposingForwardMonitor.CurrentParamters.Value;
if (initialParams.Type == TravellingType.Vehicle)
{
if(!initialReasoning.OpposingForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped)
tps.Add(initialParams);
else
{
tps.Add(initialReasoning.OpposingForwardMonitor.NaviationParameters);
distanceToGo = initial.DistanceBetween(initialReasoning.OpposingForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition, vehicleState.Front) - TahoeParams.VL;
}
}
else
tps.Add(initialReasoning.OpposingForwardMonitor.NaviationParameters);
// get params for forward lane
targetReasoning.ForwardManeuver(target, vehicleState, roadPlan, blockages, ignorable);
TravelingParameters targetParams = targetReasoning.ForwardMonitor.ParameterizationHelper(target, target,
CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ?
target.WaypointList[target.WaypointList.Count-1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle);
tps.Add(targetParams);
// ignoring vehicles add
List<int> ignoreVehicles = initialParams.VehiclesToIgnore;
ignoreVehicles.AddRange(targetParams.VehiclesToIgnore);
// decorators
List<BehaviorDecorator> decorators = !cls.Parameters.ToLeft ? TurnDecorators.RightTurnDecorator : TurnDecorators.LeftTurnDecorator;
// get final
tps.Sort();
// get the proper speed command
SpeedCommand sc = tps[0].SpeedCommand;
if (sc is StopAtDistSpeedCommand)
{
sc = new ScalarSpeedCommand(0.0);
}
// check final for stopped failed opposing
VehicleAgent forwardVa = targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle;
if (forwardVa != null)
{
// dist between
double distToFV = targetReasoning.Lane.DistanceBetween(vehicleState.Front, forwardVa.ClosestPosition);
// check stopped
bool stopped = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.5;
// check distance
bool distOk = distToFV < 2.5 * TahoeParams.VL;
// check failed
bool failed = forwardVa.QueuingState.Queuing == QueuingState.Failed;
// notify
ArbiterOutput.Output("Forward Vehicle: Stopped: " + stopped.ToString() + ", DistOk: " + distOk.ToString() + ", Failed: " + failed.ToString());
// check all for failed
if (stopped && distOk && failed)
{
// check which lane we are in
if (initial.LanePolygon.IsInside(vehicleState.Front))
{
// return to opposing lane
return new Maneuver(new HoldBrakeBehavior(), new OpposingLanesState(initial, true, CoreCommon.CorePlanningState, vehicleState), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
{
// lane state
return new Maneuver(new HoldBrakeBehavior(), new StayInLaneState(target, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
}
// continue the lane change with the proper speed command
ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, cls.Parameters.ToLeft, distanceToGo,
sc, ignoreVehicles, initial.ReversePath, target.LanePath(), initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, false),
initial.NumberOfLanesRight(vehicleState.Front, false));
// standard maneuver
return new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp);
}
#endregion
else if (cls.Parameters.Reason == LaneChangeReason.SlowForwardVehicle)
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
else
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
}
#endregion
else
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
}
#endregion
}
/// <summary>
/// Gets parameterization
/// </summary>
/// <param name="lane"></param>
/// <param name="speed"></param>
/// <param name="distance"></param>
/// <param name="state"></param>
/// <returns></returns>
public TravelingParameters GetParameters(ArbiterLane lane, double speed, double distance, VehicleState state)
{
double distanceCutOff = CoreCommon.OperationslStopLineSearchDistance;
Maneuver m = new Maneuver();
SpeedCommand sc;
bool usingSpeed = true;
#region Distance Cutoff
// check if distance is less than cutoff
if (distance < distanceCutOff)
{
// default behavior
sc = new StopAtDistSpeedCommand(distance);
Behavior b = new StayInLaneBehavior(lane.LaneId, sc, new List<int>(), lane.LanePath(), lane.Width, lane.NumberOfLanesLeft(state.Front, true), lane.NumberOfLanesRight(state.Front, true));
// stopping so not using speed param
usingSpeed = false;
// standard behavior is fine for maneuver
m = new Maneuver(b, new StayInLaneState(lane, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, state.Timestamp);
}
#endregion
#region Outisde Distance Envelope
// not inside distance envalope
else
{
// get lane
ArbiterLane al = lane;
// default behavior
sc = new ScalarSpeedCommand(speed);
Behavior b = new StayInLaneBehavior(al.LaneId, sc, new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true));
// standard behavior is fine for maneuver
m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, state.Timestamp);
}
#endregion
#region Parameterize
// create new params
TravelingParameters tp = new TravelingParameters();
tp.Behavior = m.PrimaryBehavior;
tp.Decorators = m.PrimaryBehavior.Decorators;
tp.DistanceToGo = distance;
tp.NextState = m.PrimaryState;
tp.RecommendedSpeed = speed;
tp.Type = TravellingType.Navigation;
tp.UsingSpeed = usingSpeed;
tp.SpeedCommand = sc;
tp.VehiclesToIgnore = new List<int>();
// return navigation params
return tp;
#endregion
}
/// <summary>
/// Generate the traveling parameterization for the desired behaivor
/// </summary>
/// <param name="lane"></param>
/// <param name="navStopSpeed"></param>
/// <param name="navStopDistance"></param>
/// <param name="navStop"></param>
/// <param name="navStopType"></param>
/// <param name="state"></param>
/// <returns></returns>
private TravelingParameters NavStopParameterization(ArbiterLane lane, double navStopSpeed, double navStopDistance, ArbiterWaypoint navStop, StopType navStopType, VehicleState state)
{
// get min dist
double distanceCutOff = CoreCommon.OperationalStopDistance;
// turn direction default
List<BehaviorDecorator> decorators = TurnDecorators.NoDecorators;
// create new params
TravelingParameters tp = new TravelingParameters();
#region Get Maneuver
Maneuver m = new Maneuver();
bool usingSpeed = true;
// get lane path
LinePath lp = lane.LanePath().Clone();
lp.Reverse();
#region Distance Cutoff
// check if distance is less than cutoff
if (navStopDistance < distanceCutOff)
{
// default behavior
tp.SpeedCommand = new StopAtDistSpeedCommand(navStopDistance);
Behavior b = new StayInLaneBehavior(lane.LaneId, new StopAtDistSpeedCommand(navStopDistance), new List<int>(), lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false));
// stopping so not using speed param
usingSpeed = false;
IState nextState = CoreCommon.CorePlanningState;
m = new Maneuver(b, nextState, decorators, state.Timestamp);
}
#endregion
#region Outisde Distance Envelope
// not inside distance envalope
else
{
// get lane
ArbiterLane al = lane;
// default behavior
tp.SpeedCommand = new ScalarSpeedCommand(Math.Min(navStopSpeed, 2.24));
Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(Math.Min(navStopSpeed, 2.24)), new List<int>(), lp, al.Width, al.NumberOfLanesRight(state.Front, false), al.NumberOfLanesLeft(state.Front, false));
// standard behavior is fine for maneuver
m = new Maneuver(b, CoreCommon.CorePlanningState, decorators, state.Timestamp);
}
#endregion
#endregion
#region Parameterize
tp.Behavior = m.PrimaryBehavior;
tp.Decorators = m.PrimaryBehavior.Decorators;
tp.DistanceToGo = navStopDistance;
tp.NextState = m.PrimaryState;
tp.RecommendedSpeed = navStopSpeed;
tp.Type = TravellingType.Navigation;
tp.UsingSpeed = usingSpeed;
tp.VehiclesToIgnore = new List<int>();
// return navigation params
return tp;
#endregion
}
/// <summary>
/// Behavior given we stay in the current lane
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
/// <param name="downstreamPoint"></param>
/// <returns></returns>
public TravelingParameters Primary(IFQMPlanable lane, VehicleState state, RoadPlan roadPlan,
List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, bool log)
{
// possible parameterizations
List<TravelingParameters> tps = new List<TravelingParameters>();
#region Lane Major Parameterizations with Current Lane Goal Params, If Best Goal Exists in Current Lane
// check if the best goal is in the current lane
ArbiterWaypoint lanePoint = null;
if (lane.AreaComponents.Contains(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Lane))
lanePoint = roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest;
// get the next thing we need to stop at no matter what and parameters for stopping at it
ArbiterWaypoint laneNavStop;
double laneNavStopSpeed;
double laneNavStopDistance;
StopType laneNavStopType;
this.NextNavigationalStop(lane, lanePoint, state.Front, state.ENCovariance, ignorable,
out laneNavStopSpeed, out laneNavStopDistance, out laneNavStopType, out laneNavStop);
// create parameterization of the stop
TravelingParameters laneNavParams = this.NavStopParameterization(lane, roadPlan, laneNavStopSpeed, laneNavStopDistance, laneNavStop, laneNavStopType, state);
this.navigationParameters = laneNavParams;
this.laneParameters = laneNavParams;
tps.Add(laneNavParams);
#region Log
if (log)
{
// add to current parames to arbiter information
CoreCommon.CurrentInformation.FQMBehavior = laneNavParams.Behavior.ToShortString();
CoreCommon.CurrentInformation.FQMBehaviorInfo = laneNavParams.Behavior.ShortBehaviorInformation();
CoreCommon.CurrentInformation.FQMSpeedCommand = laneNavParams.Behavior.SpeedCommandString();
CoreCommon.CurrentInformation.FQMDistance = laneNavParams.DistanceToGo.ToString("F6");
CoreCommon.CurrentInformation.FQMSpeed = laneNavParams.RecommendedSpeed.ToString("F6");
CoreCommon.CurrentInformation.FQMState = laneNavParams.NextState.ShortDescription();
CoreCommon.CurrentInformation.FQMStateInfo = laneNavParams.NextState.StateInformation();
CoreCommon.CurrentInformation.FQMStopType = laneNavStopType.ToString();
CoreCommon.CurrentInformation.FQMWaypoint = laneNavStop.ToString();
CoreCommon.CurrentInformation.FQMSegmentSpeedLimit = lane.CurrentMaximumSpeed(state.Position).ToString("F1");
}
#endregion
#endregion
#region Forward Vehicle Parameterization
// forward vehicle update
this.ForwardVehicle.Update(lane, state);
// clear current params
this.followingParameters = null;
// check not in a sparse area
bool sparseArea = lane is ArbiterLane ?
((ArbiterLane)lane).GetClosestPartition(state.Front).Type == PartitionType.Sparse :
((SupraLane)lane).ClosestComponent(state.Front) == SLComponentType.Initial && ((SupraLane)lane).Initial.GetClosestPartition(state.Front).Type == PartitionType.Sparse;
// exists forward vehicle
if (!sparseArea && this.ForwardVehicle.ShouldUseForwardTracker)
{
// get forward vehicle params, set lane decorators
TravelingParameters vehicleParams = this.ForwardVehicle.Follow(lane, state, ignorable);
vehicleParams.Behavior.Decorators.AddRange(this.laneParameters.Decorators);
this.FollowingParameters = vehicleParams;
#region Log
if (log)
{
// add to current parames to arbiter information
CoreCommon.CurrentInformation.FVTBehavior = vehicleParams.Behavior.ToShortString();
CoreCommon.CurrentInformation.FVTSpeed = this.ForwardVehicle.FollowingParameters.RecommendedSpeed.ToString("F3");
CoreCommon.CurrentInformation.FVTSpeedCommand = vehicleParams.Behavior.SpeedCommandString();
CoreCommon.CurrentInformation.FVTDistance = vehicleParams.DistanceToGo.ToString("F2");
CoreCommon.CurrentInformation.FVTState = vehicleParams.NextState.ShortDescription();
CoreCommon.CurrentInformation.FVTStateInfo = vehicleParams.NextState.StateInformation();
// set xSeparation
CoreCommon.CurrentInformation.FVTXSeparation = this.ForwardVehicle.ForwardControl.xSeparation.ToString("F0");
}
#endregion
// check if we're stopped behind fv, allow wait timer if true, stop wait timer if not behind fv
bool forwardVehicleStopped = this.ForwardVehicle.CurrentVehicle.IsStopped;
bool forwardSeparationGood = this.ForwardVehicle.ForwardControl.xSeparation < TahoeParams.VL * 2.5;
bool wereStopped = CoreCommon.Communications.GetVehicleSpeed().Value < 0.1;
bool forwardDistanceToGo = vehicleParams.DistanceToGo < 3.5;
if (forwardVehicleStopped && forwardSeparationGood && wereStopped && forwardDistanceToGo)
this.ForwardVehicle.StoppedBehindForwardVehicle = true;
else
{
this.ForwardVehicle.StoppedBehindForwardVehicle = false;
this.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Stop();
this.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Reset();
}
// add vehicle param
tps.Add(vehicleParams);
}
else
{
// no forward vehicle
this.followingParameters = null;
this.ForwardVehicle.StoppedBehindForwardVehicle = false;
}
#endregion
#region Sparse Waypoint Parameterization
// check for sparse waypoints downstream
bool sparseDownstream;
bool sparseNow;
double sparseDistance;
lane.SparseDetermination(state.Front, out sparseDownstream, out sparseNow, out sparseDistance);
// check if sparse areas downstream
if (sparseDownstream)
{
// set the distance to the sparse area
if (sparseNow)
sparseDistance = 0.0;
// get speed
double speed = SpeedTools.GenerateSpeed(sparseDistance, 2.24, lane.CurrentMaximumSpeed(state.Front));
// maneuver
Maneuver m = new Maneuver();
bool usingSpeed = true;
SpeedCommand sc = new ScalarSpeedCommand(speed);
#region Parameterize Given Speed Command
// check if lane
if (lane is ArbiterLane)
{
// get lane
ArbiterLane al = (ArbiterLane)lane;
// default behavior
Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(speed), new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true));
b.Decorators = this.laneParameters.Decorators;
// standard behavior is fine for maneuver
m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), this.laneParameters.Decorators, state.Timestamp);
}
// check if supra lane
else if (lane is SupraLane)
{
// get lane
SupraLane sl = (SupraLane)lane;
// get sl state
StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState;
// get default beheavior
Behavior b = sisls.GetBehavior(new ScalarSpeedCommand(speed), state.Front, new List<int>());
b.Decorators = this.laneParameters.Decorators;
// standard behavior is fine for maneuver
m = new Maneuver(b, sisls, this.laneParameters.Decorators, state.Timestamp);
}
#endregion
#region Parameterize
// create new params
TravelingParameters tp = new TravelingParameters();
tp.Behavior = m.PrimaryBehavior;
tp.Decorators = this.laneParameters.Decorators;
tp.DistanceToGo = Double.MaxValue;
tp.NextState = m.PrimaryState;
tp.RecommendedSpeed = speed;
tp.Type = TravellingType.Navigation;
tp.UsingSpeed = usingSpeed;
tp.SpeedCommand = sc;
tp.VehiclesToIgnore = new List<int>();
// return navigation params
tps.Add(tp);
#endregion
}
#endregion
// sort params by most urgent
tps.Sort();
// set current params
this.currentParameters = tps[0];
// get behavior to check add vehicles to ignore
if (this.currentParameters.Behavior is StayInLaneBehavior)
((StayInLaneBehavior)this.currentParameters.Behavior).IgnorableObstacles = this.ForwardVehicle.VehiclesToIgnore;
// out of navigation, blockages, and vehicle following determine the actual primary parameters for this lane
return tps[0];
}
/// <summary>
/// Plans what maneuer we should take next
/// </summary>
/// <param name="planningState"></param>
/// <param name="navigationalPlan"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicles"></param>
/// <param name="obstacles"></param>
/// <param name="blockage"></param>
/// <returns></returns>
public Maneuver Plan(IState planningState, INavigationalPlan navigationalPlan, VehicleState vehicleState,
SceneEstimatorTrackedClusterCollection vehicles, SceneEstimatorUntrackedClusterCollection obstacles, List<ITacticalBlockage> blockages)
{
#region Waiting At Intersection Exit
if (planningState is WaitingAtIntersectionExitState)
{
// state
WaitingAtIntersectionExitState waies = (WaitingAtIntersectionExitState)planningState;
// get intersection plan
IntersectionPlan ip = (IntersectionPlan)navigationalPlan;
// nullify turn reasoning
this.TurnReasoning = null;
#region Intersection Monitor Updates
// check correct intersection monitor
if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(waies.exitWaypoint.AreaSubtypeWaypointId) &&
(IntersectionTactical.IntersectionMonitor == null ||
!IntersectionTactical.IntersectionMonitor.OurMonitor.Waypoint.Equals(waies.exitWaypoint)))
{
// create new intersection monitor
IntersectionTactical.IntersectionMonitor = new IntersectionMonitor(
waies.exitWaypoint,
CoreCommon.RoadNetwork.IntersectionLookup[waies.exitWaypoint.AreaSubtypeWaypointId],
vehicleState,
ip.BestOption);
}
// update if exists
if (IntersectionTactical.IntersectionMonitor != null)
{
// update monitor
IntersectionTactical.IntersectionMonitor.Update(vehicleState);
// print current
ArbiterOutput.Output(IntersectionTactical.IntersectionMonitor.IntersectionStateString());
}
#endregion
#region Desired Behavior
// get best option from previously saved
IConnectAreaWaypoints icaw = null;
if (waies.desired != null)
{
ArbiterInterconnect tmpInterconnect = waies.desired;
if (waies.desired.InitialGeneric is ArbiterWaypoint)
{
ArbiterWaypoint init = (ArbiterWaypoint)waies.desired.InitialGeneric;
if (init.NextPartition != null && init.NextPartition.Final.Equals(tmpInterconnect.FinalGeneric))
icaw = init.NextPartition;
else
icaw = waies.desired;
}
else
icaw = waies.desired;
}
else
{
icaw = ip.BestOption;
waies.desired = icaw.ToInterconnect;
}
#endregion
#region Turn Feasibility Reasoning
// check uturn
if (waies.desired.TurnDirection == ArbiterTurnDirection.UTurn)
waies.turnTestState = TurnTestState.Completed;
// check already determined feasible
if (waies.turnTestState == TurnTestState.Unknown ||
waies.turnTestState == TurnTestState.Failed)
{
#region Determine Behavior to Accomplish Turn
// get default turn behavior
TurnBehavior testTurnBehavior = this.DefaultTurnBehavior(icaw);
// set saudi decorator
if (waies.saudi != SAUDILevel.None)
testTurnBehavior.Decorators.Add(new ShutUpAndDoItDecorator(waies.saudi));
// set to ignore all vehicles
testTurnBehavior.VehiclesToIgnore = new List<int>(new int[]{-1});
#endregion
#region Check Turn Feasible
// check if we have completed
CompletionReport turnCompletionReport;
bool completedTest = CoreCommon.Communications.TestExecute(testTurnBehavior, out turnCompletionReport);//CoreCommon.Communications.AsynchronousTestHasCompleted(testTurnBehavior, out turnCompletionReport, true);
// if we have completed the test
if(completedTest || ((TrajectoryBlockedReport)turnCompletionReport).BlockageType != BlockageType.Dynamic)
{
#region Can Complete
// check success
if (turnCompletionReport.Result == CompletionResult.Success)
{
// set completion state of the turn
waies.turnTestState = TurnTestState.Completed;
}
#endregion
#region No Saudi Level, Found Initial Blockage
// otherwise we cannot do the turn, check if saudi is still none
else if(waies.saudi == SAUDILevel.None)
{
// notify
ArbiterOutput.Output("Increased Saudi Level of Turn to L1");
// up the saudi level, set as turn failed and no other option
waies.saudi = SAUDILevel.L1;
waies.turnTestState = TurnTestState.Failed;
}
#endregion
#region Already at L1 Saudi
else if(waies.saudi == SAUDILevel.L1)
{
// notify
ArbiterOutput.Output("Turn with Saudi L1 Level failed");
// get an intersection plan without this interconnect
IntersectionPlan testPlan = CoreCommon.Navigation.PlanIntersectionWithoutInterconnect(
waies.exitWaypoint,
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId],
waies.desired);
// check that the plan exists
if (!testPlan.BestOption.ToInterconnect.Equals(waies.desired) &&
testPlan.BestRouteTime < double.MaxValue - 1.0)
{
// get the desired interconnect
ArbiterInterconnect reset = testPlan.BestOption.ToInterconnect;
#region Check that the reset interconnect is feasible
// test the reset interconnect
TurnBehavior testResetTurnBehavior = this.DefaultTurnBehavior(reset);
// set to ignore all vehicles
testResetTurnBehavior.VehiclesToIgnore = new List<int>(new int[] { -1 });
// check if we have completed
CompletionReport turnResetCompletionReport;
bool completedResetTest = CoreCommon.Communications.TestExecute(testResetTurnBehavior, out turnResetCompletionReport);
// check to see if this is feasible
if (completedResetTest && turnResetCompletionReport is SuccessCompletionReport && reset.Blockage.ProbabilityExists < 0.95)
{
// notify
ArbiterOutput.Output("Found clear interconnect: " + reset.ToString() + " adding blockage to current interconnect: " + waies.desired.ToString());
// set the interconnect as being blocked
CoreCommon.Navigation.AddInterconnectBlockage(waies.desired);
// reset all
waies.desired = reset;
waies.turnTestState = TurnTestState.Completed;
waies.saudi = SAUDILevel.None;
waies.useTurnBounds = true;
IntersectionMonitor.ResetDesired(reset);
}
#endregion
#region No Lane Bounds
// otherwise try without lane bounds
else
{
// notify
ArbiterOutput.Output("Had to fallout to using no turn bounds");
// up the saudi level, set as turn failed and no other option
waies.saudi = SAUDILevel.L1;
waies.turnTestState = TurnTestState.Completed;
waies.useTurnBounds = false;
}
#endregion
}
#region No Lane Bounds
// otherwise try without lane bounds
else
{
// up the saudi level, set as turn failed and no other option
waies.saudi = SAUDILevel.L1;
waies.turnTestState = TurnTestState.Unknown;
waies.useTurnBounds = false;
}
#endregion
}
#endregion
// want to reset ourselves
return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
#endregion
}
#endregion
#region Entry Monitor Blocked
// checks the entry monitor vehicle for failure
if (IntersectionMonitor != null && IntersectionMonitor.EntryAreaMonitor != null &&
IntersectionMonitor.EntryAreaMonitor.Vehicle != null && IntersectionMonitor.EntryAreaMonitor.Failed)
{
ArbiterOutput.Output("Entry area blocked");
// get an intersection plan without this interconnect
IntersectionPlan testPlan = CoreCommon.Navigation.PlanIntersectionWithoutInterconnect(
waies.exitWaypoint,
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId],
waies.desired,
true);
// check that the plan exists
if (!testPlan.BestOption.ToInterconnect.Equals(waies.desired) &&
testPlan.BestRouteTime < double.MaxValue - 1.0)
{
// get the desired interconnect
ArbiterInterconnect reset = testPlan.BestOption.ToInterconnect;
#region Check that the reset interconnect is feasible
// test the reset interconnect
TurnBehavior testResetTurnBehavior = this.DefaultTurnBehavior(reset);
// set to ignore all vehicles
testResetTurnBehavior.VehiclesToIgnore = new List<int>(new int[] { -1 });
// check if we have completed
CompletionReport turnResetCompletionReport;
bool completedResetTest = CoreCommon.Communications.TestExecute(testResetTurnBehavior, out turnResetCompletionReport);
// check to see if this is feasible
if (reset.TurnDirection == ArbiterTurnDirection.UTurn || (completedResetTest && turnResetCompletionReport is SuccessCompletionReport && reset.Blockage.ProbabilityExists < 0.95))
{
// notify
ArbiterOutput.Output("Found clear interconnect: " + reset.ToString() + " adding blockage to all possible turns into final");
// set all the interconnects to the final as being blocked
if (((ITraversableWaypoint)waies.desired.FinalGeneric).IsEntry)
{
foreach (ArbiterInterconnect toBlock in ((ITraversableWaypoint)waies.desired.FinalGeneric).Entries)
CoreCommon.Navigation.AddInterconnectBlockage(toBlock);
}
// check if exists previous partition to block
if (waies.desired.FinalGeneric is ArbiterWaypoint)
{
ArbiterWaypoint finWaypoint = (ArbiterWaypoint)waies.desired.FinalGeneric;
if (finWaypoint.PreviousPartition != null)
CoreCommon.Navigation.AddBlockage(finWaypoint.PreviousPartition, finWaypoint.Position, false);
}
// reset all
waies.desired = reset;
waies.turnTestState = TurnTestState.Completed;
waies.saudi = SAUDILevel.None;
waies.useTurnBounds = true;
IntersectionMonitor.ResetDesired(reset);
// want to reset ourselves
return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
#endregion
}
else
{
ArbiterOutput.Output("Entry area blocked, but no otehr valid route found");
}
}
#endregion
// check if can traverse
if (IntersectionTactical.IntersectionMonitor == null || IntersectionTactical.IntersectionMonitor.CanTraverse(icaw, vehicleState))
{
#region If can traverse the intersection
// quick check not interconnect
if (!(icaw is ArbiterInterconnect))
icaw = icaw.ToInterconnect;
// get interconnect
ArbiterInterconnect ai = (ArbiterInterconnect)icaw;
// clear all old completion reports
CoreCommon.Communications.ClearCompletionReports();
// check if uturn
if (ai.InitialGeneric is ArbiterWaypoint && ai.FinalGeneric is ArbiterWaypoint && ai.TurnDirection == ArbiterTurnDirection.UTurn)
{
// go into turn
List<ArbiterLane> involvedLanes = new List<ArbiterLane>();
involvedLanes.Add(((ArbiterWaypoint)ai.InitialGeneric).Lane);
involvedLanes.Add(((ArbiterWaypoint)ai.FinalGeneric).Lane);
uTurnState nextState = new uTurnState(((ArbiterWaypoint)ai.FinalGeneric).Lane,
IntersectionToolkit.uTurnBounds(vehicleState, involvedLanes));
nextState.Interconnect = ai;
// hold brake
Behavior b = new HoldBrakeBehavior();
// return maneuver
return new Maneuver(b, nextState, nextState.DefaultStateDecorators, vehicleState.Timestamp);
}
else
{
if (ai.FinalGeneric is ArbiterWaypoint)
{
ArbiterWaypoint finalWaypoint = (ArbiterWaypoint)ai.FinalGeneric;
// get turn params
LinePath finalPath;
LineList leftLL;
LineList rightLL;
IntersectionToolkit.TurnInfo(finalWaypoint, out finalPath, out leftLL, out rightLL);
// go into turn
IState nextState = new TurnState(ai, ai.TurnDirection, finalWaypoint.Lane, finalPath, leftLL, rightLL, new ScalarSpeedCommand(2.5), waies.saudi, waies.useTurnBounds);
// hold brake
Behavior b = new HoldBrakeBehavior();
// return maneuver
return new Maneuver(b, nextState, nextState.DefaultStateDecorators, vehicleState.Timestamp);
}
else
{
// final perimeter waypoint
ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)ai.FinalGeneric;
// get turn params
LinePath finalPath;
LineList leftLL;
LineList rightLL;
IntersectionToolkit.ZoneTurnInfo(ai, apw, out finalPath, out leftLL, out rightLL);
// go into turn
IState nextState = new TurnState(ai, ai.TurnDirection, null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(2.5), waies.saudi, waies.useTurnBounds);
// hold brake
Behavior b = new HoldBrakeBehavior();
// return maneuver
return new Maneuver(b, nextState, nextState.DefaultStateDecorators, vehicleState.Timestamp);
}
}
#endregion
}
// otherwise need to wait
else
{
IState next = waies;
return new Maneuver(new HoldBrakeBehavior(), next, next.DefaultStateDecorators, vehicleState.Timestamp);
}
}
#endregion
#region Stopping At Exit
else if (planningState is StoppingAtExitState)
{
// cast to exit stopping
StoppingAtExitState saes = (StoppingAtExitState)planningState;
saes.currentPosition = vehicleState.Front;
// get intersection plan
IntersectionPlan ip = (IntersectionPlan)navigationalPlan;
// if has an intersection
if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(saes.waypoint.AreaSubtypeWaypointId))
{
// create new intersection monitor
IntersectionTactical.IntersectionMonitor = new IntersectionMonitor(
saes.waypoint,
CoreCommon.RoadNetwork.IntersectionLookup[saes.waypoint.AreaSubtypeWaypointId],
vehicleState,
ip.BestOption);
// update it
IntersectionTactical.IntersectionMonitor.Update(vehicleState);
}
else
IntersectionTactical.IntersectionMonitor = null;
// otherwise update the stop parameters
saes.currentPosition = vehicleState.Front;
Behavior b = saes.Resume(vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value);
return new Maneuver(b, saes, saes.DefaultStateDecorators, vehicleState.Timestamp);
}
#endregion
#region In uTurn
else if (planningState is uTurnState)
{
// get state
uTurnState uts = (uTurnState)planningState;
// check if in other lane
if (CoreCommon.Communications.HasCompleted((new UTurnBehavior(null, null, null, null)).GetType()))
{
// quick check
if (uts.Interconnect != null && uts.Interconnect.FinalGeneric is ArbiterWaypoint)
{
// get the closest partition to the new lane
ArbiterLanePartition alpClose = uts.TargetLane.GetClosestPartition(vehicleState.Front);
// waypoints
ArbiterWaypoint partitionInitial = alpClose.Initial;
ArbiterWaypoint uturnEnd = (ArbiterWaypoint)uts.Interconnect.FinalGeneric;
// check initial past end
if (partitionInitial.WaypointId.Number > uturnEnd.WaypointId.Number)
{
// get waypoints inclusive
List<ArbiterWaypoint> inclusive = uts.TargetLane.WaypointsInclusive(uturnEnd, partitionInitial);
bool found = false;
// loop through
foreach (ArbiterWaypoint aw in inclusive)
{
if (!found && aw.WaypointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId))
{
// notiofy
ArbiterOutput.Output("removed checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString() + " as passed over in uturn");
// remove
CoreCommon.Mission.MissionCheckpoints.Dequeue();
// set found
found = true;
}
}
}
// default check
else if (uts.Interconnect.FinalGeneric.Equals(CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId]))
{
// notiofy
ArbiterOutput.Output("removed checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString() + " as end of uturn");
// remove
CoreCommon.Mission.MissionCheckpoints.Dequeue();
}
}
// check if the uturn is for a blockage
else if (uts.Interconnect == null)
{
// get final lane
ArbiterLane targetLane = uts.TargetLane;
// check has opposing
if (targetLane.Way.Segment.Lanes.Count > 1)
{
// check the final checkpoint is in our lane
if (CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.AreaSubtypeId.Equals(targetLane.LaneId))
{
// check that the final checkpoint is within the uturn polygon
if (uts.Polygon != null &&
uts.Polygon.IsInside(CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId].Position))
{
// remove the checkpoint
ArbiterOutput.Output("Found checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.ToString() + " inside blockage uturn area, dequeuing");
CoreCommon.Mission.MissionCheckpoints.Dequeue();
}
}
}
}
// stay in target lane
IState nextState = new StayInLaneState(uts.TargetLane, new Probability(0.8, 0.2), true, CoreCommon.CorePlanningState);
Behavior b = new StayInLaneBehavior(uts.TargetLane.LaneId, new ScalarSpeedCommand(2.0), new List<int>(), uts.TargetLane.LanePath(), uts.TargetLane.Width, uts.TargetLane.NumberOfLanesLeft(vehicleState.Front, true), uts.TargetLane.NumberOfLanesRight(vehicleState.Front, true));
return new Maneuver(b, nextState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// otherwise continue uturn
else
{
// get polygon
Polygon p = uts.Polygon;
// add polygon to observable
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(p, ArbiterInformationDisplayObjectType.uTurnPolygon));
// check the type of uturn
if (!uts.singleLaneUturn)
{
// get ending path
LinePath endingPath = uts.TargetLane.LanePath();
// next state is current
IState nextState = uts;
// behavior
Behavior b = new UTurnBehavior(p, endingPath, uts.TargetLane.LaneId, new ScalarSpeedCommand(2.0));
// maneuver
return new Maneuver(b, nextState, null, vehicleState.Timestamp);
}
else
{
// get ending path
LinePath endingPath = uts.TargetLane.LanePath().Clone();
endingPath = endingPath.ShiftLateral(-2.0);//uts.TargetLane.Width);
// add polygon to observable
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(endingPath, ArbiterInformationDisplayObjectType.leftBound));
// next state is current
IState nextState = uts;
// behavior
Behavior b = new UTurnBehavior(p, endingPath, uts.TargetLane.LaneId, new ScalarSpeedCommand(2.0));
// maneuver
return new Maneuver(b, nextState, null, vehicleState.Timestamp);
}
}
}
#endregion
#region In Turn
else if (planningState is TurnState)
{
// get state
TurnState ts = (TurnState)planningState;
// add bounds to observable
if (ts.LeftBound != null && ts.RightBound != null)
{
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(ts.LeftBound, ArbiterInformationDisplayObjectType.leftBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(ts.RightBound, ArbiterInformationDisplayObjectType.rightBound));
}
// create current turn reasoning
if(this.TurnReasoning == null)
this.TurnReasoning = new TurnReasoning(ts.Interconnect,
IntersectionTactical.IntersectionMonitor != null ? IntersectionTactical.IntersectionMonitor.EntryAreaMonitor : null);
// get primary maneuver
Maneuver primary = this.TurnReasoning.PrimaryManeuver(vehicleState, blockages, ts);
// get secondary maneuver
Maneuver? secondary = this.TurnReasoning.SecondaryManeuver(vehicleState, (IntersectionPlan)navigationalPlan);
// return the manevuer
return secondary.HasValue ? secondary.Value : primary;
}
#endregion
#region Itnersection Startup
else if (planningState is IntersectionStartupState)
{
// state and plan
IntersectionStartupState iss = (IntersectionStartupState)planningState;
IntersectionStartupPlan isp = (IntersectionStartupPlan)navigationalPlan;
// initial path
LinePath vehiclePath = new LinePath(new Coordinates[] { vehicleState.Rear, vehicleState.Front });
List<ITraversableWaypoint> feasibleEntries = new List<ITraversableWaypoint>();
// vehicle polygon forward of us
Polygon vehicleForward = vehicleState.ForwardPolygon;
// best waypoint
ITraversableWaypoint best = null;
double bestCost = Double.MaxValue;
// given feasible choose best, no feasible choose random
if (feasibleEntries.Count == 0)
{
foreach (ITraversableWaypoint itw in iss.Intersection.AllEntries.Values)
{
if (vehicleForward.IsInside(itw.Position))
{
feasibleEntries.Add(itw);
}
}
if (feasibleEntries.Count == 0)
{
foreach (ITraversableWaypoint itw in iss.Intersection.AllEntries.Values)
{
feasibleEntries.Add(itw);
}
}
}
// get best
foreach (ITraversableWaypoint itw in feasibleEntries)
{
if (isp.NodeTimeCosts.ContainsKey(itw))
{
KeyValuePair<ITraversableWaypoint, double> lookup = new KeyValuePair<ITraversableWaypoint, double>(itw, isp.NodeTimeCosts[itw]);
if (best == null || lookup.Value < bestCost)
{
best = lookup.Key;
bestCost = lookup.Value;
}
}
}
// get something going to this waypoint
ArbiterInterconnect interconnect = null;
if(best.IsEntry)
{
ArbiterInterconnect closest = null;
double closestDistance = double.MaxValue;
foreach (ArbiterInterconnect ai in best.Entries)
{
double dist = ai.InterconnectPath.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front);
if (closest == null || dist < closestDistance)
{
closest = ai;
closestDistance = dist;
}
}
interconnect = closest;
}
else if(best is ArbiterWaypoint && ((ArbiterWaypoint)best).PreviousPartition != null)
{
interconnect = ((ArbiterWaypoint)best).PreviousPartition.ToInterconnect;
}
// get state
if (best is ArbiterWaypoint)
{
// go to this turn state
LinePath finalPath;
LineList leftBound;
LineList rightBound;
IntersectionToolkit.TurnInfo((ArbiterWaypoint)best, out finalPath, out leftBound, out rightBound);
return new Maneuver(new HoldBrakeBehavior(), new TurnState(interconnect, interconnect.TurnDirection, ((ArbiterWaypoint)best).Lane,
finalPath, leftBound, rightBound, new ScalarSpeedCommand(2.0)), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
{
// go to this turn state
LinePath finalPath;
LineList leftBound;
LineList rightBound;
IntersectionToolkit.ZoneTurnInfo(interconnect, (ArbiterPerimeterWaypoint)best, out finalPath, out leftBound, out rightBound);
return new Maneuver(new HoldBrakeBehavior(), new TurnState(interconnect, interconnect.TurnDirection, null,
finalPath, leftBound, rightBound, new ScalarSpeedCommand(2.0)), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
#region Unknown
else
{
throw new Exception("Unknown planning state in intersection tactical plan: " + planningState.ToString());
}
#endregion
}
