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