/// <summary>
/// Updates the blockage component
/// </summary>
public List<ITacticalBlockage> DetermineBlockages(IState currentState)
{
// check for a timeout or cooldown
if (timeoutTimer.ElapsedMilliseconds / 1000.0 > 0.5 ||
(cooldownTimer.IsRunning && cooldownTimer.ElapsedMilliseconds < cooldownMaxValue))
{
// reset all if recent blockage timed out
this.Reset();
}
// check cooldown timer expired
else if (cooldownTimer.IsRunning && cooldownTimer.ElapsedMilliseconds > cooldownMaxValue)
{
cooldownTimer.Stop();
cooldownTimer.Reset();
cooldownMaxValue = 0.0;
}
// set the report to use for this
TrajectoryBlockedReport tbr = this.recentReport;
// check if we still have a blockage report
if (tbr != null)
{
// check type of state is stay in lane or stay in supra lane
if (currentState is StayInLaneState || currentState is StayInSupraLaneState)
{
// check type of completion report matches current state
if (tbr.BehaviorType == (new StayInLaneBehavior(null, null, null)).GetType() ||
tbr.BehaviorType == (new SupraLaneBehavior(null, null, 0, 0, 0, null, null, 0, 0, 0, null, null, null)).GetType())
{
// create the lane blockage
LaneBlockage lb = new LaneBlockage(tbr);
CoreCommon.CurrentInformation.Blockage = lb.ToString();
// return the blockage
return new List<ITacticalBlockage>(new ITacticalBlockage[] { lb });
}
}
// check if we're in a turn state
else if (currentState is TurnState)
{
// check type of completion report matches current state
if (tbr.BehaviorType == (new TurnBehavior(null, null, null, null, null, null)).GetType())
{
// create a new turn blockage
TurnBlockage tb = new TurnBlockage(tbr);
CoreCommon.CurrentInformation.Blockage = tb.ToString();
// return the blockage
return new List<ITacticalBlockage>(new ITacticalBlockage[] { tb });
}
}
// check if we're changing lanes
else if (currentState is ChangeLanesState)
{
// check type of completion report matches current state
if (tbr.BehaviorType == (new ChangeLaneBehavior(null, null, false, 0.0, null, null)).GetType())
{
// create a new turn blockage
LaneChangeBlockage lcb = new LaneChangeBlockage(tbr);
CoreCommon.CurrentInformation.Blockage = lcb.ToString();
// return the blockage
return new List<ITacticalBlockage>(new ITacticalBlockage[] { lcb });
}
}
// check if we are recovering from a blockage
else if (currentState is BlockageRecoveryState)
{
// check type
if (((BlockageRecoveryState)currentState).RecoveryBehavior != null &&
tbr.BehaviorType == ((BlockageRecoveryState)currentState).RecoveryBehavior.GetType())
{
// create a new blockage recovery blockage
BlockageRecoveryBlockage brb = new BlockageRecoveryBlockage(tbr);
CoreCommon.CurrentInformation.Blockage = brb.ToString();
((BlockageRecoveryState)currentState).RecoveryStatus = BlockageRecoverySTATUS.BLOCKED;
// return the blockage
return new List<ITacticalBlockage>(new ITacticalBlockage[] { brb });
}
}
// check if we are in an opposing lane
else if (currentState is OpposingLanesState)
{
// create a new blockage recovery blockage
OpposingLaneBlockage olb = new OpposingLaneBlockage(tbr);
CoreCommon.CurrentInformation.Blockage = olb.ToString();
// return the blockage
return new List<ITacticalBlockage>(new ITacticalBlockage[] { olb });
}
}
// fall out returns empty list of blockages
return new List<ITacticalBlockage>();
}
/// <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;
}
