/// <summary>
/// Returns if we should pass or nor
/// </summary>
/// <returns></returns>
public bool ShouldPass(out LaneChangeInformation lci)
{
// passing reason set to none by default
lci = new LaneChangeInformation(LaneChangeReason.NotApplicable, this.CurrentVehicle);
// check the queuing state of the forward vehicle
if (this.CurrentVehicle.QueuingState.Queuing == QueuingState.Failed)
{
lci = new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, this.CurrentVehicle);
return(true);
}
else
{
// check if moving much too slow for the next stop for long period of time
#warning need to implement slow moving vehicle pass after beta
// for now return false
return(false);
}
}
/// <summary>
/// Checks if we should pass the forward vehicle
/// </summary>
/// <param name="lci"></param>
/// <param name="lane"></param>
/// <returns></returns>
public bool ShouldPass(out LaneChangeInformation lci, ArbiterLane lane)
{
// passing reason set to none by default
lci = new LaneChangeInformation(LaneChangeReason.NotApplicable, this.CurrentVehicle);
// check the queuing state of the forward vehicle
if (this.CurrentVehicle.QueuingState.Queuing == QueuingState.Failed)
{
lci = new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, this.CurrentVehicle);
return(true);
}
// check inside any safety zone
foreach (ArbiterSafetyZone asz in lane.SafetyZones)
{
if (asz.IsInSafety(this.CurrentVehicle.ClosestPosition))
{
return(false);
}
}
foreach (ArbiterIntersection ai in CoreCommon.RoadNetwork.ArbiterIntersections.Values)
{
if (ai.IntersectionPolygon.IsInside(this.CurrentVehicle.ClosestPosition))
{
return(false);
}
}
if ((this.CurrentVehicle.Speed < CoreCommon.Communications.GetVehicleSpeed().Value ||
(this.CurrentVehicle.IsStopped && this.CurrentVehicle.StateMonitor.Observed.speedValid)) &&
this.CurrentVehicle.Speed < 0.7 * lane.Way.Segment.SpeedLimits.MaximumSpeed)
{
lci = new LaneChangeInformation(LaneChangeReason.SlowForwardVehicle, this.CurrentVehicle);
return(true);
}
// fall out
return(false);
}
public bool ForwardClear(VehicleState state, double usDistanceToTravel, double usAvgSpeed, LaneChangeInformation information, Coordinates minReturn)
{
if (information.Reason == LaneChangeReason.FailedForwardVehicle)
{
return(this.ForwardMonitor.ClearForDisabledVehiclePass(lane, state, CoreCommon.Communications.GetVehicleSpeed().Value, minReturn));
}
else
{
throw new Exception("standard forward clear in opposing lateral reasoning not imp");
}
}
public bool ForwardClear(VehicleState state, double usDistanceToTravel, double usAvgSpeed, LaneChangeInformation information, Coordinates minReturn)
{
if (information.Reason == LaneChangeReason.FailedForwardVehicle)
return this.ForwardMonitor.ClearForDisabledVehiclePass(lane, state, CoreCommon.Communications.GetVehicleSpeed().Value, minReturn);
else
throw new Exception("standard forward clear in opposing lateral reasoning not imp");
}
/// <summary>
/// Behavior we would like to do other than going straight
/// </summary>
/// <param name="arbiterLane"></param>
/// <param name="vehicleState"></param>
/// <param name="p"></param>
/// <param name="blockages"></param>
/// <returns></returns>
/// <remarks>tries to go right, if not goest left if needs
/// to if forward vehicle ahead and we're stopped because of them</remarks>
public Maneuver?SecondaryManeuver(ArbiterLane arbiterLane, ArbiterLane closestGood, VehicleState vehicleState, List <ITacticalBlockage> blockages,
LaneChangeParameters?entryParameters)
{
// check blockages
if (blockages != null && blockages.Count > 0 && blockages[0] is OpposingLaneBlockage)
{
// 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));
}
// check dist needed to complete
double neededDistance = (Math.Abs(arbiterLane.LaneId.Number - closestGood.LaneId.Number) * 1.5 * TahoeParams.VL) +
(-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration));
// get upper bound
LinePath.PointOnPath xFront = arbiterLane.LanePath().GetClosestPoint(vehicleState.Front);
Coordinates xUpper = arbiterLane.LanePath().AdvancePoint(xFront, -neededDistance).Location;
if (entryParameters.HasValue)
{
// check if we should get back, keep speed nice n' lowi fpassing failed
if (entryParameters.Value.Reason == LaneChangeReason.FailedForwardVehicle)
{
double xToReturn = arbiterLane.DistanceBetween(entryParameters.Value.DefaultReturnLowerBound, vehicleState.Front);
if (xToReturn >= 0.0)
{
ArbiterOutput.Output("Distance until must return to lane: " + xToReturn);
}
else
{
ArbiterOutput.Output("Can return to lane from arbitrary upper bound: " + xToReturn);
}
// check can return
if (xToReturn < 0)
{
// check if right lateral exists exactly here
if (this.rightLateralReasoning.ExistsExactlyHere(vehicleState) && this.rightLateralReasoning.LateralLane.Equals(closestGood))
{
ArbiterOutput.Output("Right lateral reasoning good and exists exactly here");
return(this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, true));
}
else if (!this.rightLateralReasoning.ExistsRelativelyHere(vehicleState) && !this.rightLateralReasoning.LateralLane.Equals(closestGood))
{
ArbiterOutput.Output("Right lateral reasoning not good closest and does not exist here");
if (this.secondaryLateralReasoning == null || !this.secondaryLateralReasoning.LateralLane.Equals(closestGood))
{
this.secondaryLateralReasoning = new LateralReasoning(closestGood, UrbanChallenge.Common.Sensors.SideObstacleSide.Passenger);
}
if (this.secondaryLateralReasoning.ExistsExactlyHere(vehicleState))
{
ILateralReasoning tmpReasoning = this.rightLateralReasoning;
this.rightLateralReasoning = this.secondaryLateralReasoning;
Maneuver?tmp = this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, true);
this.rightLateralReasoning = tmpReasoning;
return(tmp);
}
else
{
ArbiterOutput.Output("Cosest good lane does not exist here??");
return(null);
}
}
else
{
ArbiterOutput.Output("Can't change lanes!!, RL exists exactly: " + this.rightLateralReasoning.ExistsExactlyHere(vehicleState).ToString() +
", RL exists rel: " + this.rightLateralReasoning.ExistsRelativelyHere(vehicleState).ToString() + ", RL closest good: " + this.rightLateralReasoning.LateralLane.Equals(closestGood).ToString());
return(null);
}
}
else
{
return(null);
}
}
}
// lane change info
LaneChangeInformation lci = new LaneChangeInformation(LaneChangeReason.Navigation, null);
// notify
ArbiterOutput.Output("In Opposing with no Previous state knowledge, attempting to return");
// check if right lateral exists exactly here
if (this.rightLateralReasoning.ExistsExactlyHere(vehicleState) && this.rightLateralReasoning.LateralLane.Equals(closestGood))
{
ArbiterOutput.Output("Right lateral reasoning good and exists exactly here");
return(this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, false));
}
else if (!this.rightLateralReasoning.ExistsRelativelyHere(vehicleState) && !this.rightLateralReasoning.LateralLane.Equals(closestGood))
{
ArbiterOutput.Output("Right lateral reasoning not good closest and does not exist here");
if (this.secondaryLateralReasoning == null || !this.secondaryLateralReasoning.LateralLane.Equals(closestGood))
{
this.secondaryLateralReasoning = new LateralReasoning(closestGood, UrbanChallenge.Common.Sensors.SideObstacleSide.Passenger);
}
if (this.secondaryLateralReasoning.ExistsExactlyHere(vehicleState))
{
ILateralReasoning tmpReasoning = this.rightLateralReasoning;
this.rightLateralReasoning = this.secondaryLateralReasoning;
Maneuver?tmp = this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, false);
this.rightLateralReasoning = tmpReasoning;
return(tmp);
}
else
{
ArbiterOutput.Output("Cosest good lane does not exist here??");
return(null);
}
}
else
{
ArbiterOutput.Output("Can't change lanes!!, RL exists exactly: " + this.rightLateralReasoning.ExistsExactlyHere(vehicleState).ToString() +
", RL exists rel: " + this.rightLateralReasoning.ExistsRelativelyHere(vehicleState).ToString() + ", RL closest good: " + this.rightLateralReasoning.LateralLane.Equals(closestGood).ToString());
return(null);
}
}
public bool ForwardClear(VehicleState state, double usDistanceToTravel, double usAvgSpeed, LaneChangeInformation information, Coordinates minReturn)
{
throw new Exception("The method or operation is not implemented.");
}
public bool ForwardClear(VehicleState state, double usDistanceToTravel, double usAvgSpeed, LaneChangeInformation information, Coordinates minReturn)
{
throw new Exception("The method or operation is not implemented.");
}
/// <summary>
/// Distinctly want to make lane change, parameters for doing so
/// </summary>
/// <param name="arbiterLane"></param>
/// <param name="left"></param>
/// <param name="vehicleState"></param>
/// <param name="roadPlan"></param>
/// <param name="blockages"></param>
/// <param name="ignorable"></param>
/// <returns></returns>
public Maneuver? LaneChangeManeuver(ArbiterLane lane, bool left, ArbiterWaypoint goal, VehicleState vehicleState,
List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, LaneChangeInformation laneChangeInformation, Maneuver? secondary,
out LaneChangeParameters parameters)
{
// distance until the change is complete
double distanceToUpperBound = lane.DistanceBetween(vehicleState.Front, goal.Position);
double neededDistance = (1.5 * TahoeParams.VL * Math.Max(Math.Abs(goal.Lane.LaneId.Number - lane.LaneId.Number), 1)) +
(-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration));
parameters = new LaneChangeParameters();
if (distanceToUpperBound < neededDistance)
return null;
Coordinates upperBound = new Coordinates();
Coordinates upperReturnBound = new Coordinates();
Coordinates minimumReturnBound = new Coordinates();
Coordinates defaultReturnBound = new Coordinates();
if (laneChangeInformation.Reason == LaneChangeReason.FailedForwardVehicle)
{
double distToForwards = Math.Min(neededDistance, lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) - 2.0);
upperBound = lane.LanePath().AdvancePoint(lane.LanePath().GetClosestPoint(vehicleState.Front), distToForwards).Location;
defaultReturnBound = lane.LanePath().AdvancePoint(lane.LanePath().GetClosestPoint(this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition), TahoeParams.VL * 4.0).Location;
}
// get params for lane change
LaneChangeParameters? changeParams = this.LaneChangeParameterization(
laneChangeInformation,
lane, left ? lane.LaneOnLeft : lane.LaneOnRight, false, goal.Position, upperBound,
new Coordinates(), new Coordinates(), defaultReturnBound, blockages, ignorable,
vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value);
// set lane change params
parameters = changeParams.HasValue ? changeParams.Value : parameters = new LaneChangeParameters();
// check if the lane change is available or recommended
if (changeParams != null && changeParams.Value.Feasible)
{
// minimize parameterizations
List<TravelingParameters> tps = new List<TravelingParameters>();
tps.Add(this.ForwardMonitor.LaneParameters);
tps.Add(changeParams.Value.Parameters);
if(this.ForwardMonitor.FollowingParameters.HasValue)
tps.Add(this.ForwardMonitor.FollowingParameters.Value);
tps.Sort();
// check if possible to make lane change
if (changeParams.Value.Available)
{
// get traveling params from FQM to make sure we stopped for vehicle, behind vehicle
double v = CoreCommon.Communications.GetVehicleSpeed().Value;
// just use other params with shorted distance bound
TravelingParameters final = tps[0];
// final behavior
ChangeLaneBehavior clb = new ChangeLaneBehavior(
lane.LaneId,
parameters.Target.LaneId,
left,
final.DistanceToGo,
final.SpeedCommand,
final.VehiclesToIgnore,
lane.LanePath(),
parameters.Target.LanePath(),
lane.Width,
parameters.Target.Width,
lane.NumberOfLanesLeft(vehicleState.Front, true), lane.NumberOfLanesRight(vehicleState.Front, true));
// final state
ChangeLanesState cls = new ChangeLanesState(changeParams.Value);
// return maneuver
return new Maneuver(clb, cls, left ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator, vehicleState.Timestamp);
}
// otherwise plan for requirements of change coming up
else
{
// check if secondary exists
if (secondary != null)
{
return secondary;
}
// otherwise plan for upcoming
else
{
// get params
TravelingParameters final = tps[0];
// return maneuver
return new Maneuver(tps[0].Behavior, tps[0].NextState, this.ForwardMonitor.NavigationParameters.Decorators, vehicleState.Timestamp);
}
}
}
// return null over fallout
return null;
}
/// <summary>
/// Generates the lane change parameterization
/// </summary>
/// <param name="information"></param>
/// <param name="initial"></param>
/// <param name="final"></param>
/// <param name="goal"></param>
/// <param name="departUpperBound"></param>
/// <param name="defaultReturnLowerBound"></param>
/// <param name="minimumReturnComplete"></param>
/// <param name="defaultReturnUpperBound"></param>
/// <param name="blockages"></param>
/// <param name="ignorable"></param>
/// <param name="state"></param>
/// <param name="speed"></param>
/// <returns></returns>
public LaneChangeParameters? LaneChangeParameterization(LaneChangeInformation information, ArbiterLane initial, ArbiterLane target,
bool targetOncoming, Coordinates goal, Coordinates departUpperBound, Coordinates defaultReturnLowerBound, Coordinates minimumReturnComplete,
Coordinates defaultReturnUpperBound, List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, VehicleState state, double speed)
{
// check if the target lane exists here
bool validTarget = target.LanePath().GetClosestPoint(state.Front).Location.DistanceTo(state.Front) < 17 && target.IsInside(state.Front);
// params
bool toLeft = initial.LaneOnLeft != null ? initial.LaneOnLeft.Equals(target) || (targetOncoming && !initial.Way.WayId.Equals(target.Way.WayId)) : false;
// get appropriate lateral reasoning
ILateralReasoning lateralReasoning = toLeft ? this.leftLateralReasoning : this.rightLateralReasoning;
#region Target Lane Valid Here
// check if the target is currently valid
if (validTarget)
{
// lane change parameterizations
List<LaneChangeParameters> lcps = new List<LaneChangeParameters>();
// distance to the current goal (means different things for all)
double xGoal = initial.DistanceBetween(state.Front, goal);
// get next stop
List<WaypointType> types = new List<WaypointType>();
types.Add(WaypointType.Stop);
types.Add(WaypointType.End);
ArbiterWaypoint nextMajor = initial.GetNext(state.Front, types, ignorable);
double xLaneMajor = initial.DistanceBetween(state.Front, nextMajor.Position);
xGoal = Math.Min(xGoal, xLaneMajor);
#region Failed Vehicle Lane Change
if (information.Reason == LaneChangeReason.FailedForwardVehicle)
{
#region Target Opposing
// check if target lane backwards
if (targetOncoming)
{
// available and feasible
bool avail = false;
bool feas = false;
// check if min return distance < goal distance
double xReturnMin = initial.DistanceBetween(state.Front, minimumReturnComplete);
double xDepart = initial.DistanceBetween(state.Front, departUpperBound);
// dist to upper bound along lane and dist to end of adjacent lane
double adjLaneDist = initial.DistanceBetween(state.Front, minimumReturnComplete);
// this is feasible
feas = xGoal > xReturnMin ? true : false;
// check if not feasible that the goal is the current checkpoint
if (!feas && CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId].Position.Equals(goal))
feas = true;
// check adj and rear clear
bool adjRearClear = lateralReasoning.AdjacentAndRearClear(state);
// check if forwards clear
bool frontClear = lateralReasoning.ForwardClear(state, xReturnMin, 2.24, information, minimumReturnComplete);
Console.WriteLine("Adjacent, Rear: " + adjRearClear.ToString() + ", Forward: " + frontClear.ToString());
// if clear
if (frontClear && adjRearClear)
{
// notify
ArbiterOutput.Output("Lane Change Params: Target Oncoming Failed Vehicle: Adjacent, Rear, and Front Clear");
// available
avail = true;
// get lateral parameterization
TravelingParameters lateralParams = this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane, goal,
state.Front, CoreCommon.CorePlanningState, state, lateralReasoning.ForwardVehicle(state));
// change into the opposing lane wih opposing forward parameterization
LaneChangeParameters lcp = new LaneChangeParameters(avail, feas, initial, false, target, targetOncoming, toLeft, null,
xDepart, null, toLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator, lateralParams,
departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound, information.Reason);
// we have been forced
lcp.ForcedOpposing = true;
// return created params
return lcp;
}
// fell through for some reason, return parameterization explaining why
LaneChangeParameters fallThroughParams = new LaneChangeParameters(avail, feas, initial, false, target, targetOncoming, toLeft, null,
xDepart, null, toLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator, this.ForwardMonitor.LaneParameters,
departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound, information.Reason);
// return fall through parameters
return fallThroughParams;
}
#endregion
#region Target Forwards
// otherwise target lane forwards
else
{
// check lateral clear and initial lane does not run out
if (lateralReasoning.AdjacentAndRearClear(state) && !initial.GetClosestPoint(defaultReturnUpperBound).Equals(initial.LanePath().EndPoint))
{
// notify
ArbiterOutput.Output("Lane Change Params: Failed Vehicle Target Forwards: Adjacent and Rear Clear");
// dist to upper bound along lane and dist to end of adjacent lane
double distToReturn = initial.DistanceBetween(state.Front, defaultReturnUpperBound);
double adjLaneDist = initial.DistanceBetween(state.Front, target.LanePath().EndPoint.Location);
// check enough lane room to pass
if (distToReturn < adjLaneDist && distToReturn <= initial.DistanceBetween(state.Front, goal))
{
// check enough room to change lanes
ArbiterWaypoint nextTargetMajor = target.GetNext(state.Front, types, ignorable);
double xTargetLaneMajor = initial.DistanceBetween(state.Front, nextTargetMajor.Position);
// check dist needed to complete
double neededDistance = (1.5 * TahoeParams.VL * Math.Abs(initial.LaneId.Number - target.LaneId.Number)) +
(-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration));
// check return dist
if (distToReturn < xTargetLaneMajor && neededDistance <= xTargetLaneMajor)
{
// parameters for traveling in current lane to hit other
List<TravelingParameters> tps = new List<TravelingParameters>();
// update lateral
((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.Update(target, state);
// check lateral reasoning for forward vehicle badness
if (!((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker ||
!((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped ||
initial.DistanceBetween(state.Front, ((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) >= distToReturn)
{
// get parameterization for lateral lane
TravelingParameters navParams = this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane,
goal, state.Front, CoreCommon.CorePlanningState, state, lateralReasoning.ForwardVehicle(state));
tps.Add(navParams);
// get and add the vehicle parameterization for our lane
if (this.ForwardMonitor.FollowingParameters.HasValue)
tps.Add(this.ForwardMonitor.FollowingParameters.Value);
// get final
tps.Sort();
TravelingParameters final = tps[0];
// check final distance > needed
if (navParams.DistanceToGo > neededDistance)
{
// set ignorable vhcs
final.VehiclesToIgnore = this.ForwardMonitor.FollowingParameters.HasValue ? this.ForwardMonitor.FollowingParameters.Value.VehiclesToIgnore : new List<int>();
if (((LateralReasoning)lateralReasoning).ForwardMonitor.FollowingParameters.HasValue)
final.VehiclesToIgnore.AddRange(((LateralReasoning)lateralReasoning).ForwardMonitor.FollowingParameters.Value.VehiclesToIgnore);
// parameterize
LaneChangeParameters lcp = new LaneChangeParameters();
lcp.Decorators = TurnDecorators.RightTurnDecorator;
lcp.Available = true;
lcp.Feasible = true;
lcp.Parameters = final;
lcp.Initial = initial;
lcp.InitialOncoming = false;
lcp.Target = target;
lcp.TargetOncoming = false;
lcp.Reason = LaneChangeReason.FailedForwardVehicle;
lcp.DefaultReturnLowerBound = defaultReturnLowerBound;
lcp.DefaultReturnUpperBound = defaultReturnUpperBound;
lcp.DepartUpperBound = departUpperBound;
lcp.MinimumReturnComplete = minimumReturnComplete;
return lcp;
}
}
}
}
}
// otherwise infeasible
return null;
}
#endregion
}
#endregion
#region Navigation Lane Change
else if (information.Reason == LaneChangeReason.Navigation)
{
// parameters for traveling in current lane to hit other
List<TravelingParameters> tps = new List<TravelingParameters>();
// get navigation parameterization
TravelingParameters lateralParams = this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane,
goal, state.Front, CoreCommon.CorePlanningState, state, lateralReasoning.ForwardVehicle(state));
tps.Add(lateralParams);
// get and add the nav parameterization relative to our lane
tps.Add(this.ForwardMonitor.NavigationParameters);
// check avail
bool adjRearAvailable = lateralReasoning.AdjacentAndRearClear(state);
// if they are available we are in good shape, need to slow for nav, forward vehicles
if (adjRearAvailable)
{
// notify
ArbiterOutput.Output("Lane Change Params: Navigation: Adjacent and Rear Clear");
#region Check Forward and Lateral Vehicles
if (this.ForwardMonitor.CurrentParameters.Type == TravellingType.Vehicle && lateralParams.Type == TravellingType.Vehicle)
{
// check enough room to change lanes
ArbiterWaypoint nextTargetMajor = target.GetNext(state.Front, types, ignorable);
double xTargetLaneMajor = initial.DistanceBetween(state.Front, nextTargetMajor.Position);
// distnace to goal
double goalDist = initial.DistanceBetween(state.Front, goal);
// check dist needed to complete
double neededDistance = (1.5 * TahoeParams.VL * Math.Abs(initial.LaneId.Number - target.LaneId.Number)) +
(-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration));
// check for proper distances
if (xTargetLaneMajor >= neededDistance && goalDist >= neededDistance && this.ForwardMonitor.NavigationParameters.DistanceToGo >= neededDistance)
{
// check distance to return (weeds out safety zone crap
Coordinates lateralVehicle = ((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition;
double distToReturn = initial.DistanceBetween(state.Front, initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(lateralVehicle), 30.0).Location);
// check passing params
LaneChangeInformation lci;
bool shouldPass = ((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.ShouldPass(out lci);
// check passing params
LaneChangeInformation lciInit;
bool shouldPassInit = this.ForwardMonitor.ForwardVehicle.ShouldPass(out lciInit);
// check forward lateral stopped and enough distance to go around and not vehicles between it and goal close enough to stop
if(shouldPass && lci.Reason == LaneChangeReason.FailedForwardVehicle && goalDist > distToReturn &&
(!shouldPassInit || lciInit.Reason != LaneChangeReason.FailedForwardVehicle || this.ForwardMonitor.CurrentParameters.DistanceToGo > lateralParams.DistanceToGo + TahoeParams.VL * 5))
{
// return that we should pass it as normal in the initial lane
return null;
}
// check get distance to upper
double xUpper = this.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped ? Math.Min(goalDist, neededDistance) : this.ForwardMonitor.ForwardVehicle.ForwardControl.xSeparation - 2;
Coordinates upper = initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(state.Front), xUpper).Location;
// add current params if not stopped
if (!this.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped)
tps.Add(this.ForwardMonitor.CurrentParameters);
// get final
tps.Sort();
TravelingParameters final = tps[0];
// parameterize
LaneChangeParameters lcp = new LaneChangeParameters(true, true, initial, false, target, false, toLeft,
null, final.DistanceToGo - 3.0, null, toLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator,
final, upper, new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.Navigation);
return lcp;
}
}
#endregion
#region Check Forward Vehicle
else if (this.ForwardMonitor.CurrentParameters.Type == TravellingType.Vehicle)
{
// check enough room to change lanes
ArbiterWaypoint nextTargetMajor = target.GetNext(state.Front, types, ignorable);
double xTargetLaneMajor = initial.DistanceBetween(state.Front, nextTargetMajor.Position);
// distnace to goal
double goalDist = initial.DistanceBetween(state.Front, goal);
// check dist needed to complete
double neededDistance = (1.5 * TahoeParams.VL * Math.Abs(initial.LaneId.Number - target.LaneId.Number)) +
(-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration));
// check for proper distances
if (xTargetLaneMajor >= neededDistance && goalDist >= neededDistance && this.ForwardMonitor.NavigationParameters.DistanceToGo >= neededDistance)
{
// add current params if not stopped
if(!this.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped)
tps.Add(this.ForwardMonitor.CurrentParameters);
// get final
tps.Sort();
TravelingParameters final = tps[0];
// check get distance to upper
double xUpper = this.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped ? neededDistance : this.ForwardMonitor.ForwardVehicle.ForwardControl.xSeparation - 2;
Coordinates upper = initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(state.Front), xUpper).Location;
// parameterize
LaneChangeParameters lcp = new LaneChangeParameters(true, true, initial, false, target, false, toLeft,
null, final.DistanceToGo - 3.0, null, toLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator,
final, upper, new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.Navigation);
return lcp;
}
}
#endregion
#region Lateral Vehicle
// check to see if should use the forward tracker, i.e. forward vehicle exists
else if (lateralParams.Type == TravellingType.Vehicle)
{
// add current params
tps.Add(this.ForwardMonitor.CurrentParameters);
// check enough room to change lanes
ArbiterWaypoint nextTargetMajor = target.GetNext(state.Front, types, ignorable);
double xTargetLaneMajor = initial.DistanceBetween(state.Front, nextTargetMajor.Position);
// distnace to goal
double goalDist = initial.DistanceBetween(state.Front, goal);
// check dist needed to complete
double neededDistance = (1.5 * TahoeParams.VL * Math.Abs(initial.LaneId.Number - target.LaneId.Number)) +
(-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration));
// check for proper distances
if (xTargetLaneMajor >= neededDistance && goalDist >= neededDistance && this.ForwardMonitor.NavigationParameters.DistanceToGo >= neededDistance)
{
// check distance to return (weeds out safety zone crap
Coordinates lateralVehicle = ((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition;
double distToReturn = initial.DistanceBetween(state.Front, initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(lateralVehicle), 30.0).Location);
// check passing params
LaneChangeInformation lci;
bool shouldPass = ((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.ShouldPass(out lci);
// check forward lateral stopped and enough distance to go around and not vehicles between it and goal close enough to stop
if (shouldPass && lci.Reason == LaneChangeReason.FailedForwardVehicle && goalDist > distToReturn)
{
// return that we should pass it as normal in the initial lane
return null;
}
// check if we are already slowed for this vehicle and are at a good distance from it
if (speed < lateralParams.RecommendedSpeed + 1.0)
{
// get final
tps.Sort();
TravelingParameters final = tps[0];
// upper bound is nav bound
Coordinates upper = initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(state.Front), Math.Min(neededDistance, final.DistanceToGo)).Location;
// parameterize
LaneChangeParameters lcp = new LaneChangeParameters(true, true, initial, false, target, false, toLeft,
null, final.DistanceToGo - 3.0, null, TurnDecorators.LeftTurnDecorator, final, upper, new Coordinates(),
new Coordinates(), new Coordinates(), LaneChangeReason.Navigation);
return lcp;
}
// otherwise need to slow for it or other
else
{
// get final
tps.Sort();
TravelingParameters final = tps[0];
// upper bound is nav bound
Coordinates upper = initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(state.Front), final.DistanceToGo).Location;
// parameterize
LaneChangeParameters lcp = new LaneChangeParameters(false, true, initial, false, target, false, toLeft,
null, final.DistanceToGo - 3.0, null, TurnDecorators.LeftTurnDecorator, final, new Coordinates(), new Coordinates(),
new Coordinates(), new Coordinates(), LaneChangeReason.Navigation);
return lcp;
}
}
}
#endregion
#region No forward or lateral
// otherwise just go!
else
{
// add current params
tps.Add(this.ForwardMonitor.CurrentParameters);
// check enough room to change lanes
ArbiterWaypoint nextTargetMajor = target.GetNext(state.Front, types, ignorable);
double xTargetLaneMajor = initial.DistanceBetween(state.Front, nextTargetMajor.Position);
// distnace to goal
double goalDist = initial.DistanceBetween(state.Front, goal);
// check dist needed to complete
double neededDistance = (1.5 * TahoeParams.VL * Math.Abs(initial.LaneId.Number - target.LaneId.Number)) +
(-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration));
// check for proper distances
if (xTargetLaneMajor >= neededDistance && goalDist >= neededDistance && this.ForwardMonitor.NavigationParameters.DistanceToGo >= neededDistance)
{
// get final
tps.Sort();
TravelingParameters final = tps[0];
// upper bound is nav bound
Coordinates upper = initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(state.Front), Math.Min(neededDistance, final.DistanceToGo)).Location;
// parameterize
LaneChangeParameters lcp = new LaneChangeParameters(true, true, initial, false, target, false, toLeft,
null, final.DistanceToGo, null, TurnDecorators.LeftTurnDecorator, final, upper, new Coordinates(),
new Coordinates(), new Coordinates(), LaneChangeReason.Navigation);
// return the parameterization
return lcp;
}
}
#endregion
}
// otherwise we need to determine how to make this available
else
{
// notify
ArbiterOutput.Output("Lane Change Params: Navigation Adjacent and Rear NOT Clear");
// gets the adjacent vehicle
VehicleAgent adjacent = lateralReasoning.AdjacentVehicle;
// add current params
tps.Add(this.ForwardMonitor.CurrentParameters);
#region Pass Adjacent
// checks if it is failed for us to pass it
if (adjacent != null && (adjacent.IsStopped || adjacent.Speed < 1.5))
{
// get final
List<TravelingParameters> adjacentTravelingParams = new List<TravelingParameters>();
adjacentTravelingParams.Add(this.ForwardMonitor.CurrentParameters);
adjacentTravelingParams.Add(this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane, goal, state.Front, CoreCommon.CorePlanningState, state, null));
adjacentTravelingParams.Sort();
//tps.Sort();
TravelingParameters final = adjacentTravelingParams[0];// tps[0];
// parameterize
LaneChangeParameters lcp = new LaneChangeParameters();
lcp.Available = false;
lcp.Feasible = true;
lcp.Parameters = final;
return lcp;
}
#endregion
#region Follow Adjacent
// otherwise we need to follow it, as it is lateral, this means 0.0 speed
else if (adjacent != null)
{
// get and add the vehicle parameterization relative to our lane
TravelingParameters tmp = new TravelingParameters();
tmp.Behavior = new StayInLaneBehavior(initial.LaneId, new ScalarSpeedCommand(0.0), this.ForwardMonitor.CurrentParameters.VehiclesToIgnore,
initial.LanePath(), initial.Width, initial.NumberOfLanesLeft(state.Front, true), initial.NumberOfLanesRight(state.Front, true));
tmp.NextState = CoreCommon.CorePlanningState;
// parameterize
LaneChangeParameters lcp = new LaneChangeParameters();
lcp.Available = false;
lcp.Feasible = true;
lcp.Parameters = tmp;
return lcp;
}
#endregion
#region Wait for the rear vehicle
else
{
TravelingParameters tp = new TravelingParameters();
tp.SpeedCommand = new ScalarSpeedCommand(0.0);
tp.UsingSpeed = true;
tp.DistanceToGo = 0.0;
tp.VehiclesToIgnore = new List<int>();
tp.RecommendedSpeed = 0.0;
tp.NextState = CoreCommon.CorePlanningState;
tp.Behavior = new StayInLaneBehavior(initial.LaneId, tp.SpeedCommand, new List<int>(), initial.LanePath(), initial.Width, initial.NumberOfLanesLeft(state.Front, true), initial.NumberOfLanesRight(state.Front, true));
// parameterize
LaneChangeParameters lcp = new LaneChangeParameters();
lcp.Available = false;
lcp.Feasible = true;
lcp.Parameters = tp;
return lcp;
}
#endregion
}
}
#endregion
#region Passing Lane Change
else if (information.Reason == LaneChangeReason.SlowForwardVehicle)
{
throw new Exception("passing slow vehicles not yet supported");
}
#endregion
// fallout returns null
return null;
}
#endregion
#region Target Lane Not Valid, Plan Navigation
// otherwise plan for when we approach target if this is a navigational change
else if(information.Reason == LaneChangeReason.Navigation)
{
// parameters for traveling in current lane to hit other
List<TravelingParameters> tps = new List<TravelingParameters>();
// add current params
tps.Add(this.ForwardMonitor.CurrentParameters);
// distance between front of car and start of lane
if (target.RelativelyInside(state.Front) ||
initial.DistanceBetween(state.Front, target.LanePath().StartPoint.Location) > 0)
{
#region Vehicle and Navigation
// check to see if we're not looped around wierd
if (lateralReasoning.LateralLane.LanePath().GetClosestPoint(state.Front).Equals(lateralReasoning.LateralLane.LanePath().StartPoint))
{
// initialize the forward tracker in the other lane
ForwardVehicleTracker fvt = new ForwardVehicleTracker();
fvt.Update(lateralReasoning.LateralLane, state);
// check to see if should use the forward tracker
if (fvt.ShouldUseForwardTracker)
{
// get navigation parameterization
TravelingParameters navParams = this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane,
goal, state.Front, CoreCommon.CorePlanningState, state, lateralReasoning.ForwardVehicle(state));
tps.Add(navParams);
}
else
{
// get navigation parameterization
TravelingParameters navParams = this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane,
goal, state.Front, CoreCommon.CorePlanningState, state, null);
tps.Add(navParams);
}
}
#endregion
#region Navigation
// check to see that nav point is downstream of us
else if (initial.DistanceBetween(state.Front, goal) > 0.0)
{
// get navigation parameterization
TravelingParameters navParams = this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane,
goal, state.Front, CoreCommon.CorePlanningState, state, null);
tps.Add(navParams);
}
#endregion
else
{
return null;
}
}
else
return null;
// get final
tps.Sort();
TravelingParameters final = tps[0];
// parameterize
LaneChangeParameters lcp = new LaneChangeParameters();
lcp.Available = false;
lcp.Feasible = true;
lcp.Parameters = final;
return lcp;
}
#endregion
// fallout return null
return null;
}
/// <summary>
/// Plans a lane change from the initial lane to the adjacent lane
/// </summary>
/// <param name="initial"></param>
/// <param name="adjacent"></param>
/// <param name="vehicleState"></param>
/// <param name="forwardVehicle"></param>
/// <returns></returns>
public Maneuver? PlanLaneChange(ArbiterLane initial, ArbiterWaypoint goal, LateralReasoning adjacent, VehicleState vehicleState, VehicleAgent forwardVehicle, LaneChangeInformation lci)
{
bool toLeft = initial.LaneOnLeft != null && adjacent.LateralLane.Equals(initial.LaneOnLeft);
LaneChangeParameters updatedLCP;
return this.LaneChangeManeuver(initial, toLeft, goal, vehicleState, new List<ITacticalBlockage>(), new List<ArbiterWaypoint>(), lci, null, out updatedLCP);
}
/// <summary>
/// Distinctly want to make lane change, parameters for doing so
/// </summary>
/// <param name="arbiterLane"></param>
/// <param name="left"></param>
/// <param name="vehicleState"></param>
/// <param name="roadPlan"></param>
/// <param name="blockages"></param>
/// <param name="ignorable"></param>
/// <returns></returns>
public Maneuver? AdvancedDesiredLaneChangeManeuver(ArbiterLane lane, bool left, ArbiterWaypoint goal, RoadPlan rp, VehicleState vehicleState,
List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, LaneChangeInformation laneChangeInformation, Maneuver? secondary,
out LaneChangeParameters parameters)
{
// set aprams
parameters = new LaneChangeParameters();
// get final maneuver
Maneuver? final = null;
// check partition is not a startup chute
if (lane.GetClosestPartition(vehicleState.Front).Type != PartitionType.Startup)
{
// get the lane goal distance
double distanceToLaneGoal = lane.DistanceBetween(vehicleState.Front, goal.Position);
// check if our distance is less than 50m to the goal
if (distanceToLaneGoal < 50.0)
{
// use old
final = this.LaneChangeManeuver(lane, left, goal, vehicleState, blockages, ignorable, laneChangeInformation, secondary, out parameters);
try
{
// check final null
if (final == null)
{
// check for checkpoint within 4VL of front of failed vehicle
ArbiterCheckpoint acCurrecnt = CoreCommon.Mission.MissionCheckpoints.Peek();
if (acCurrecnt.WaypointId is ArbiterWaypointId)
{
// get waypoint
ArbiterWaypoint awCheckpoint = (ArbiterWaypoint)CoreCommon.RoadNetwork.ArbiterWaypoints[acCurrecnt.WaypointId];
// check way
if (awCheckpoint.Lane.Way.Equals(lane.Way))
{
// distance to wp
double distToWp = lane.DistanceBetween(vehicleState.Front, awCheckpoint.Position);
// check close to waypoint and stopped
if (CoreCommon.Communications.GetVehicleSpeed().Value < 0.1 && distToWp < TahoeParams.VL * 1.0)
{
ArbiterOutput.Output("Removing checkpoint: " + acCurrecnt.WaypointId.ToString() + " Stopped next to it");
CoreCommon.Mission.MissionCheckpoints.Dequeue();
return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
}
}
}
catch (Exception) { }
}
// no forward vehicle
else if (this.ForwardMonitor.ForwardVehicle.CurrentVehicle == null)
{
// adjacent monitor
LateralReasoning adjacent = null;
if (left && this.leftLateralReasoning is LateralReasoning && this.leftLateralReasoning.Exists)
{
// update
adjacent = (LateralReasoning)this.leftLateralReasoning;
}
else if (!left && this.rightLateralReasoning is LateralReasoning && this.rightLateralReasoning.Exists)
{
// update
adjacent = (LateralReasoning)this.rightLateralReasoning;
}
// check adj
if (adjacent != null)
{
// update
adjacent.ForwardMonitor.Primary(adjacent.LateralLane, vehicleState, rp, new List<ITacticalBlockage>(), new List<ArbiterWaypoint>(), false);
if (adjacent.ForwardMonitor.ForwardVehicle.CurrentVehicle == null && adjacent.AdjacentAndRearClear(vehicleState))
{
// use old
final = this.LaneChangeManeuver(lane, left, goal, vehicleState, blockages, ignorable, laneChangeInformation, secondary, out parameters);
}
}
}
}
if (!final.HasValue)
{
if (!secondary.HasValue)
{
List<TravelingParameters> falloutParams = new List<TravelingParameters>();
TravelingParameters t1 = this.ForwardMonitor.ParameterizationHelper(lane, lane, goal.Position, vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, null);
falloutParams.Add(t1);
falloutParams.Add(this.ForwardMonitor.LaneParameters);
if (this.ForwardMonitor.FollowingParameters.HasValue)
falloutParams.Add(this.ForwardMonitor.FollowingParameters.Value);
falloutParams.Sort();
TravelingParameters tpCatch = falloutParams[0];
return new Maneuver(tpCatch.Behavior, tpCatch.NextState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
{
return secondary;
}
}
else
{
return final;
}
}
/// <summary>
/// Checks if we should pass the forward vehicle
/// </summary>
/// <param name="lci"></param>
/// <param name="lane"></param>
/// <returns></returns>
public bool ShouldPass(out LaneChangeInformation lci, ArbiterLane lane)
{
// passing reason set to none by default
lci = new LaneChangeInformation(LaneChangeReason.NotApplicable, this.CurrentVehicle);
// check the queuing state of the forward vehicle
if (this.CurrentVehicle.QueuingState.Queuing == QueuingState.Failed)
{
lci = new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, this.CurrentVehicle);
return true;
}
// check inside any safety zone
foreach (ArbiterSafetyZone asz in lane.SafetyZones)
{
if (asz.IsInSafety(this.CurrentVehicle.ClosestPosition))
return false;
}
foreach (ArbiterIntersection ai in CoreCommon.RoadNetwork.ArbiterIntersections.Values)
{
if (ai.IntersectionPolygon.IsInside(this.CurrentVehicle.ClosestPosition))
return false;
}
if ((this.CurrentVehicle.Speed < CoreCommon.Communications.GetVehicleSpeed().Value ||
(this.CurrentVehicle.IsStopped && this.CurrentVehicle.StateMonitor.Observed.speedValid)) &&
this.CurrentVehicle.Speed < 0.7 * lane.Way.Segment.SpeedLimits.MaximumSpeed)
{
lci = new LaneChangeInformation(LaneChangeReason.SlowForwardVehicle, this.CurrentVehicle);
return true;
}
// fall out
return false;
}
/// <summary>
/// Returns if we should pass or nor
/// </summary>
/// <returns></returns>
public bool ShouldPass(out LaneChangeInformation lci)
{
// passing reason set to none by default
lci = new LaneChangeInformation(LaneChangeReason.NotApplicable, this.CurrentVehicle);
// check the queuing state of the forward vehicle
if (this.CurrentVehicle.QueuingState.Queuing == QueuingState.Failed)
{
lci = new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, this.CurrentVehicle);
return true;
}
else
{
// check if moving much too slow for the next stop for long period of time
#warning need to implement slow moving vehicle pass after beta
// for now return false
return false;
}
}
/// <summary>
/// Behavior we would like to do other than going straight
/// </summary>
/// <param name="arbiterLane"></param>
/// <param name="vehicleState"></param>
/// <param name="p"></param>
/// <param name="blockages"></param>
/// <returns></returns>
/// <remarks>tries to go right, if not goest left if needs
/// to if forward vehicle ahead and we're stopped because of them</remarks>
public Maneuver? SecondaryManeuver(ArbiterLane arbiterLane, ArbiterLane closestGood, VehicleState vehicleState, List<ITacticalBlockage> blockages,
LaneChangeParameters? entryParameters)
{
// check blockages
if (blockages != null && blockages.Count > 0 && blockages[0] is OpposingLaneBlockage)
{
// 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);
}
// check dist needed to complete
double neededDistance = (Math.Abs(arbiterLane.LaneId.Number - closestGood.LaneId.Number) * 1.5 * TahoeParams.VL) +
(-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration));
// get upper bound
LinePath.PointOnPath xFront = arbiterLane.LanePath().GetClosestPoint(vehicleState.Front);
Coordinates xUpper = arbiterLane.LanePath().AdvancePoint(xFront, -neededDistance).Location;
if (entryParameters.HasValue)
{
// check if we should get back, keep speed nice n' lowi fpassing failed
if (entryParameters.Value.Reason == LaneChangeReason.FailedForwardVehicle)
{
double xToReturn = arbiterLane.DistanceBetween(entryParameters.Value.DefaultReturnLowerBound, vehicleState.Front);
if(xToReturn >= 0.0)
ArbiterOutput.Output("Distance until must return to lane: " + xToReturn);
else
ArbiterOutput.Output("Can return to lane from arbitrary upper bound: " + xToReturn);
// check can return
if (xToReturn < 0)
{
// check if right lateral exists exactly here
if (this.rightLateralReasoning.ExistsExactlyHere(vehicleState) && this.rightLateralReasoning.LateralLane.Equals(closestGood))
{
ArbiterOutput.Output("Right lateral reasoning good and exists exactly here");
return this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, true);
}
else if (!this.rightLateralReasoning.ExistsRelativelyHere(vehicleState) && !this.rightLateralReasoning.LateralLane.Equals(closestGood))
{
ArbiterOutput.Output("Right lateral reasoning not good closest and does not exist here");
if (this.secondaryLateralReasoning == null || !this.secondaryLateralReasoning.LateralLane.Equals(closestGood))
this.secondaryLateralReasoning = new LateralReasoning(closestGood, UrbanChallenge.Common.Sensors.SideObstacleSide.Passenger);
if (this.secondaryLateralReasoning.ExistsExactlyHere(vehicleState))
{
ILateralReasoning tmpReasoning = this.rightLateralReasoning;
this.rightLateralReasoning = this.secondaryLateralReasoning;
Maneuver? tmp = this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, true);
this.rightLateralReasoning = tmpReasoning;
return tmp;
}
else
{
ArbiterOutput.Output("Cosest good lane does not exist here??");
return null;
}
}
else
{
ArbiterOutput.Output("Can't change lanes!!, RL exists exactly: " + this.rightLateralReasoning.ExistsExactlyHere(vehicleState).ToString() +
", RL exists rel: " + this.rightLateralReasoning.ExistsRelativelyHere(vehicleState).ToString() + ", RL closest good: " + this.rightLateralReasoning.LateralLane.Equals(closestGood).ToString());
return null;
}
}
else
return null;
}
}
// lane change info
LaneChangeInformation lci = new LaneChangeInformation(LaneChangeReason.Navigation, null);
// notify
ArbiterOutput.Output("In Opposing with no Previous state knowledge, attempting to return");
// check if right lateral exists exactly here
if (this.rightLateralReasoning.ExistsExactlyHere(vehicleState) && this.rightLateralReasoning.LateralLane.Equals(closestGood))
{
ArbiterOutput.Output("Right lateral reasoning good and exists exactly here");
return this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, false);
}
else if (!this.rightLateralReasoning.ExistsRelativelyHere(vehicleState) && !this.rightLateralReasoning.LateralLane.Equals(closestGood))
{
ArbiterOutput.Output("Right lateral reasoning not good closest and does not exist here");
if (this.secondaryLateralReasoning == null || !this.secondaryLateralReasoning.LateralLane.Equals(closestGood))
this.secondaryLateralReasoning = new LateralReasoning(closestGood, UrbanChallenge.Common.Sensors.SideObstacleSide.Passenger);
if (this.secondaryLateralReasoning.ExistsExactlyHere(vehicleState))
{
ILateralReasoning tmpReasoning = this.rightLateralReasoning;
this.rightLateralReasoning = this.secondaryLateralReasoning;
Maneuver? tmp = this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, false);
this.rightLateralReasoning = tmpReasoning;
return tmp;
}
else
{
ArbiterOutput.Output("Cosest good lane does not exist here??");
return null;
}
}
else
{
ArbiterOutput.Output("Can't change lanes!!, RL exists exactly: " + this.rightLateralReasoning.ExistsExactlyHere(vehicleState).ToString() +
", RL exists rel: " + this.rightLateralReasoning.ExistsRelativelyHere(vehicleState).ToString() + ", RL closest good: " + this.rightLateralReasoning.LateralLane.Equals(closestGood).ToString());
return null;
}
}