/// <summary>
/// Checks if hte opposing lane is clear to pass an opposing vehicle
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
/// <returns></returns>
public bool ClearForDisabledVehiclePass(ArbiterLane lane, VehicleState state, double vUs, Coordinates minReturn)
{
// update the forward vehicle
this.ForwardVehicle.Update(lane, state);
// check if the rear vehicle exists and is moving along with us
if (this.ForwardVehicle.ShouldUseForwardTracker && this.ForwardVehicle.CurrentVehicle != null)
{
// distance from other to us
double currentDistance = lane.DistanceBetween(this.ForwardVehicle.CurrentVehicle.ClosestPosition, state.Front) - (2 * TahoeParams.VL);
double minChangeDist = lane.DistanceBetween(minReturn, state.Front);
// check if he's within min return dist
if(currentDistance > minChangeDist)
{
// params
double vOther = this.ForwardVehicle.CurrentVehicle.StateMonitor.Observed.speedValid ? this.ForwardVehicle.CurrentVehicle.Speed : lane.Way.Segment.SpeedLimits.MaximumSpeed;
// get distance of envelope for him to slow to our speed
double xEnvelope = (Math.Pow(vUs, 2.0) - Math.Pow(vOther, 2.0)) / (2.0 * -0.5);
// check to see if vehicle is outside of the envelope to slow down for us after 3 seconds
double xSafe = currentDistance - minChangeDist - (xEnvelope + (vOther * 15.0));
return xSafe > 0 ? true : false;
}
else
return false;
}
else
return true;
}
/// <summary>
/// Helps with parameterizations for lateral reasoning
/// </summary>
/// <param name="referenceLane"></param>
/// <param name="fqmLane"></param>
/// <param name="goal"></param>
/// <param name="vehicleFront"></param>
/// <returns></returns>
public TravelingParameters ParameterizationHelper(ArbiterLane referenceLane, ArbiterLane fqmLane,
Coordinates goal, Coordinates vehicleFront, IState nextState, VehicleState state, VehicleAgent va)
{
// get traveling parameterized list
List<TravelingParameters> tps = new List<TravelingParameters>();
// get distance to the goal
double goalDistance;
double goalSpeed;
this.StoppingParams(goal, referenceLane, vehicleFront, new double[] { }, out goalSpeed, out goalDistance);
tps.Add(this.GetParameters(referenceLane, goalSpeed, goalDistance, state));
// get next stop
ArbiterWaypoint stopPoint;
double stopSpeed;
double stopDistance;
StopType stopType;
this.NextLaneStop(fqmLane, vehicleFront, new double[] { }, new List<ArbiterWaypoint>(),
out stopPoint, out stopSpeed, out stopDistance, out stopType);
this.StoppingParams(stopPoint.Position, referenceLane, vehicleFront, new double[] { },
out stopSpeed, out stopDistance);
tps.Add(this.GetParameters(referenceLane, stopSpeed, stopDistance, state));
// get vehicle
if (va != null)
{
VehicleAgent tmp = this.ForwardVehicle.CurrentVehicle;
double tmpDist = this.ForwardVehicle.currentDistance;
this.ForwardVehicle.CurrentVehicle = va;
this.ForwardVehicle.currentDistance = referenceLane.DistanceBetween(state.Front, va.ClosestPosition);
TravelingParameters tp = this.ForwardVehicle.Follow(referenceLane, state, new List<ArbiterWaypoint>());
tps.Add(tp);
this.ForwardVehicle.CurrentVehicle = tmp;
this.ForwardVehicle.currentDistance = tmpDist;
}
// parameterize
tps.Sort();
return tps[0];
}
/// <summary>
/// Given vehicles and there locations determines if the lane adjacent to us is occupied adjacent to the vehicle
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
/// <returns></returns>
public bool Occupied(ArbiterLane lane, VehicleState state)
{
// check stopwatch time for proper elapsed
if (this.LateralClearStopwatch.ElapsedMilliseconds / 1000.0 > 10)
this.Reset();
if (TacticalDirector.VehicleAreas.ContainsKey(lane))
{
// vehicles in the lane
List<VehicleAgent> laneVehicles = TacticalDirector.VehicleAreas[lane];
// position of the center of our vehicle
Coordinates center = state.Front - state.Heading.Normalize(TahoeParams.VL / 2.0);
// cutoff for allowing vehicles outside this range
double dCutOff = TahoeParams.VL * 1.5;
// loop through vehicles
foreach (VehicleAgent va in laneVehicles)
{
// vehicle high level distance
double d = Math.Abs(lane.DistanceBetween(center, va.ClosestPosition));
// check less than distance cutoff
if (d < dCutOff)
{
ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " filled with vehicle: " + va.ToString());
this.CurrentVehicle = va;
this.Reset();
return true;
}
}
}
// now check the lateral sensor for being occupied
if (this.SideSickObstacleDetected(lane, state))
{
this.CurrentVehicle = new VehicleAgent(true, true);
this.Reset();
ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " SIDE OBSTACLE DETECTED");
return true;
}
// none found
this.CurrentVehicle = null;
// if none found, timer not running start timer
if (!this.LateralClearStopwatch.IsRunning)
{
this.CurrentVehicle = new VehicleAgent(true, true);
this.Reset();
this.LateralClearStopwatch.Start();
ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " Clear, starting cooldown");
return true;
}
// check enough time
else if (this.LateralClearStopwatch.IsRunning && this.LateralClearStopwatch.ElapsedMilliseconds / 1000.0 > 0.5)
{
this.CurrentVehicle = null;
ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " Clear, cooldown complete");
return false;
}
// not enough time
else
{
this.CurrentVehicle = new VehicleAgent(true, true);
double timer = this.LateralClearStopwatch.ElapsedMilliseconds / 1000.0;
ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " Clear, cooldown timer: " + timer.ToString("F2"));
return true;
}
}
/// <summary>
/// Maneuver for recovering from a lane blockage, used for lane changes as well
/// </summary>
/// <param name="lane"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicleSpeed"></param>
/// <param name="defcon"></param>
/// <param name="saudi"></param>
/// <returns></returns>
public Maneuver LaneRecoveryManeuver(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed,
INavigationalPlan plan, BlockageRecoveryState brs, bool failedVehiclesPersistentlyOnly, out bool waitForUTurn)
{
// get the blockage
ITacticalBlockage laneBlockageReport = brs.EncounteredState.TacticalBlockage;
// get the turn state
StayInLaneState sils = new StayInLaneState(lane, CoreCommon.CorePlanningState);
// set wait false
waitForUTurn = false;
#region Reverse
// check the state of the recovery for being the initial state
if (brs.Defcon == BlockageRecoveryDEFCON.INITIAL)
{
// determine if the reverse behavior is recommended
if (laneBlockageReport.BlockageReport.ReverseRecommended)
{
// notify
ArbiterOutput.Output("Initial encounter, reverse recommended, reversing");
// get reverse behavior
StayInLaneBehavior reverseRecovery = this.LaneReverseRecovery(lane, vehicleState, vehicleSpeed, brs.EncounteredState.TacticalBlockage.BlockageReport);
reverseRecovery.TimeStamp = vehicleState.Timestamp;
// get recovery state
BlockageRecoveryState brsT = new BlockageRecoveryState(
reverseRecovery, null, new StayInLaneState(lane, CoreCommon.CorePlanningState),
brs.Defcon = BlockageRecoveryDEFCON.REVERSE, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
brsT.CompletionState = brsT;
// reset blockages
BlockageHandler.SetDefaultBlockageCooldown();
// maneuver
return new Maneuver(reverseRecovery, brsT, TurnDecorators.HazardDecorator, vehicleState.Timestamp);
}
}
#endregion
#region uTurn
// check if uturn is available
ArbiterLane opp = this.tacticalUmbrella.roadTactical.forwardReasoning.GetClosestOpposing(lane, vehicleState);
// resoning
OpposingLateralReasoning olrTmp = new OpposingLateralReasoning(opp, SideObstacleSide.Driver);
if (opp != null && olrTmp.ExistsExactlyHere(vehicleState) && opp.IsInside(vehicleState.Front) && opp.LanePath().GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front) < TahoeParams.VL * 3.0)
{
// check possible to reach goal given block partition way
RoadPlan uTurnNavTest = CoreCommon.Navigation.PlanRoadOppositeWithoutPartition(
lane.GetClosestPartition(vehicleState.Front),
opp.GetClosestPartition(vehicleState.Front),
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId],
true,
vehicleState.Front,
true);
// flag to try the uturn
bool uturnTry = false;
// all polygons to include
List<Polygon> allPolys = BlockageTactical.AllForwardPolygons(vehicleState, failedVehiclesPersistentlyOnly);// .AllObstacleAndStoppedVehiclePolygons(vehicleState);
// test blockage takes up segment
double numLanesLeft = lane.NumberOfLanesLeft(vehicleState.Front, true);
double numLanesRight = lane.NumberOfLanesRight(vehicleState.Front, true);
LinePath leftBound = lane.LanePath().ShiftLateral((lane.Width * numLanesLeft) + (lane.Width / 2.0));
LinePath rightBound = lane.LanePath().ShiftLateral((lane.Width * numLanesRight) + (lane.Width / 2.0));
bool segmentBlockage = BlockageTester.TestBlockage(allPolys, leftBound, rightBound);
uturnTry = segmentBlockage;
// check if we should try the uturn
if(uturnTry)
{
// check uturn timer
if(uTurnTimer.ElapsedMilliseconds / 1000.0 > 2.0)
{
#region Determine Checkpoint
// check route feasible
if (uTurnNavTest.BestPlan.laneWaypointOfInterest.RouteTime < double.MaxValue - 1
&& uTurnNavTest.BestPlan.laneWaypointOfInterest.TimeCostToPoint < double.MaxValue - 1)
{
ArbiterOutput.Output("valid route to checkpoint by rerouting, uturning");
}
// otherwise remove the next checkpoint
else
{
// notiify
ArbiterOutput.Output("NO valid route to checkpoint by rerouting");
// get the next cp
ArbiterCheckpoint cp1 = CoreCommon.Mission.MissionCheckpoints.Peek();
// get distance to blockage
double blockageDistance = 15.0;
// check cp is in our lane
if (cp1.WaypointId.AreaSubtypeId.Equals(lane.LaneId))
{
// check distance to cp1
double distanceToCp1 = lane.DistanceBetween(vehicleState.Front, CoreCommon.RoadNetwork.ArbiterWaypoints[cp1.WaypointId].Position);
// check that this is an already inserted waypoint
if (cp1.Type == CheckpointType.Inserted)
{
// remove cp
ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as inserted type of checkpoint");
ArbiterCheckpoint ac2 = CoreCommon.Mission.MissionCheckpoints.Dequeue();
ArbiterOutput.Output("removed checkpoint: " + ac2.WaypointId.ToString() + " as blocked type of checkpoint");
}
// closer to us than the blockage
else if (distanceToCp1 < blockageDistance)
{
// remove cp
ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as between us and the blockage ~ 15m away");
}
// very close to blockage on the other side
else if (distanceToCp1 < blockageDistance + 5.0)
{
// remove cp
ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as on the other side of blockage ~ 15m away");
}
// further away from the blockage
else
{
// check over all checkpoints if there exists a checkpoint cp2 in the opposing lane within 5m along our lane
ArbiterCheckpoint cp2 = null;
foreach (ArbiterWaypoint oppAw in opp.WaypointList)
{
// check checkpoint
if (oppAw.IsCheckpoint)
{
// distance between us and that waypoint
double distanceToCp2 = lane.DistanceBetween(vehicleState.Front, oppAw.Position);
// check along distance < 5.0m
if (Math.Abs(distanceToCp1 - distanceToCp2) < 5.0)
{
// set cp
cp2 = new ArbiterCheckpoint(oppAw.CheckpointId, oppAw.WaypointId, CheckpointType.Inserted);
}
}
}
// check close cp exists
if (cp2 != null)
{
// remove cp1 and replace with cp2
ArbiterOutput.Output("inserting checkpoint: " + cp2.WaypointId.ToString() + " before checkpoint: " + cp1.WaypointId.ToString() + " as can be replaced with adjacent opposing cp2");
//CoreCommon.Mission.MissionCheckpoints.Dequeue();
// get current checkpoints
ArbiterCheckpoint[] updatedAcs = new ArbiterCheckpoint[CoreCommon.Mission.MissionCheckpoints.Count + 1];
updatedAcs[0] = cp2;
CoreCommon.Mission.MissionCheckpoints.CopyTo(updatedAcs, 1);
updatedAcs[1].Type = CheckpointType.Blocked;
CoreCommon.Mission.MissionCheckpoints = new Queue<ArbiterCheckpoint>(new List<ArbiterCheckpoint>(updatedAcs));
}
// otherwise remove cp1
else
{
// remove cp
ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as cp not further down our lane");
}
}
}
// otherwise we remove the checkpoint
else
{
// remove cp
ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as cp not further down our lane");
}
}
#endregion
#region Plan Uturn
// notify
ArbiterOutput.Output("Segment blocked, uturn available");
// nav penalties
ArbiterLanePartition alpClose = lane.GetClosestPartition(vehicleState.Front);
CoreCommon.Navigation.AddHarshBlockageAcrossSegment(alpClose, vehicleState.Front);
// uturn
LinePath lpTmp = new LinePath(new Coordinates[] { vehicleState.Front, opp.LanePath().GetClosestPoint(vehicleState.Front).Location });
Coordinates vector = lpTmp[1] - lpTmp[0];
lpTmp[1] = lpTmp[1] + vector.Normalize(opp.Width / 2.0);
lpTmp[0] = lpTmp[0] - vector.Normalize(lane.Width / 2.0);
LinePath lb = lpTmp.ShiftLateral(15.0);
LinePath rb = lpTmp.ShiftLateral(-15.0);
List<Coordinates> uturnPolyCOords = new List<Coordinates>();
uturnPolyCOords.AddRange(lb);
uturnPolyCOords.AddRange(rb);
uTurnState uts = new uTurnState(opp, Polygon.GrahamScan(uturnPolyCOords));
// reset blockages
BlockageHandler.SetDefaultBlockageCooldown();
// reset the timers
this.Reset();
// go to uturn
return new Maneuver(uts.Resume(vehicleState, 2.0), uts, TurnDecorators.NoDecorators, vehicleState.Timestamp);
#endregion
}
// uturn timer not enough
else
{
// check timer running
if(!uTurnTimer.IsRunning)
{
uTurnTimer.Stop();
uTurnTimer.Reset();
uTurnTimer.Start();
}
// if gets here, need to wait
double time = uTurnTimer.ElapsedMilliseconds / 1000.0;
ArbiterOutput.Output("uTurn behavior evaluated to success, cooling down for: " + time.ToString("f2") + " out of 2");
waitForUTurn = true;
return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
else
{
waitForUTurn = false;
this.Reset();
}
}
else
{
waitForUTurn = false;
this.Reset();
}
#endregion
#region Recovery Escalation
// plan forward reasoning
this.tacticalUmbrella.roadTactical.forwardReasoning.ForwardManeuver(lane, vehicleState, (RoadPlan)plan, new List<ITacticalBlockage>(), new List<ArbiterWaypoint>());
// check clear on right, or if it does nto exist here
ILateralReasoning rightLateral = this.tacticalUmbrella.roadTactical.rightLateralReasoning;
bool rightClear = !rightLateral.Exists || !rightLateral.ExistsExactlyHere(vehicleState) ||
rightLateral.AdjacentAndRearClear(vehicleState);
// check clear on left, or if it does nto exist here
ILateralReasoning leftLateral = this.tacticalUmbrella.roadTactical.leftLateralReasoning;
bool leftClear = !leftLateral.Exists || !leftLateral.ExistsExactlyHere(vehicleState) ||
leftLateral.AdjacentAndRearClear(vehicleState);
if(leftClear && leftLateral is OpposingLateralReasoning)
leftClear = leftLateral.ForwardClear(vehicleState, TahoeParams.VL * 3.0, 2.24,
new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, null),
lane.LanePath().AdvancePoint(lane.LanePath().GetClosestPoint(vehicleState.Front), TahoeParams.VL * 3.0).Location);
// check both clear to widen
bool widenOk = rightClear && leftClear;
// Notify
ArbiterOutput.Output("Blockage tactical recovery escalation: rightClear: " + rightClear.ToString() + ", leftCler: " + leftClear.ToString());
// if we can't widen for some reason just go through with no widen
StayInLaneBehavior silb = this.LaneRecoveryBehavior(lane, vehicleState, vehicleSpeed, plan, brs, brs.EncounteredState);
silb.TimeStamp = vehicleState.Timestamp;
// check widen
if (widenOk)
{
// output
ArbiterOutput.Output("Lane Blockage Recovery: Adjacent areas clear");
// mini icrease width
silb.LaneWidth = silb.LaneWidth + TahoeParams.T;
// check execute none saudi
CompletionReport l0Cr;
bool l0TestOk = CoreCommon.Communications.TestExecute(silb, out l0Cr);
// check mini ok
if (l0TestOk)
{
// notify
ArbiterOutput.Output("Lane Blockage Recovery: Test Tahoe.T lane widen ok");
// update the current state
BlockageRecoveryState brsL0 = new BlockageRecoveryState(
silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
return new Maneuver(silb, brsL0, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
{
// notify
ArbiterOutput.Output("Lane Blockage Recovery: Test Tahoe.T lane widen failed, moving to large widen");
#region Change Lanes
// check not in change lanes
if (brs.Defcon != BlockageRecoveryDEFCON.CHANGELANES_FORWARD && brs.Defcon != BlockageRecoveryDEFCON.WIDENBOUNDS)
{
// check normal change lanes reasoning
bool shouldWait;
IState laneChangeCompletionState;
ChangeLaneBehavior changeLanesBehavior = this.ChangeLanesRecoveryBehavior(lane, vehicleState, out shouldWait, out laneChangeCompletionState);
// check change lanes behaviore exists
if (changeLanesBehavior != null)
{
ArbiterOutput.Output("Lane Blockage Recovery: Found adjacent lane available change lanes beahvior: " + changeLanesBehavior.ToShortString() + ", " + changeLanesBehavior.ShortBehaviorInformation());
// update the current state
BlockageRecoveryState brsCL = new BlockageRecoveryState(
changeLanesBehavior, laneChangeCompletionState, sils, BlockageRecoveryDEFCON.CHANGELANES_FORWARD, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
return new Maneuver(changeLanesBehavior, brsCL, changeLanesBehavior.ChangeLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator, vehicleState.Timestamp);
}
// check should wait
else if (shouldWait)
{
ArbiterOutput.Output("Lane Blockage Recovery: Should wait for the forward lane, waiting");
return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
// notify
ArbiterOutput.Output("Lane Blockage Recovery: Fell Through Forward Change Lanes");
// increase width
silb.LaneWidth = silb.LaneWidth * 3.0;
// check execute l1
CompletionReport l1Cr;
bool l1TestOk = CoreCommon.Communications.TestExecute(silb, out l1Cr);
// check ok
if (l1TestOk)
{
// notify
ArbiterOutput.Output("Lane Blockage Recovery: Test 3LW lane large widen ok");
// update the current state
BlockageRecoveryState brsL1 = new BlockageRecoveryState(
silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
return new Maneuver(silb, brsL1, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// go to l2 for all the marbles
else
{
// notify
ArbiterOutput.Output("Lane Blockage Recovery: Test 3LW lane large widen failed, moving to 3LW, L1 Saudi");
ShutUpAndDoItDecorator saudi2 = new ShutUpAndDoItDecorator(SAUDILevel.L1);
List<BehaviorDecorator> saudi2bds = new List<BehaviorDecorator>(new BehaviorDecorator[] { saudi2 });
silb.Decorators = saudi2bds;
BlockageRecoveryState brsL2 = new BlockageRecoveryState(
silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
return new Maneuver(silb, brsL2, saudi2bds, vehicleState.Timestamp);
}
}
}
#endregion
// fallout goes back to lane state
return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
/// <summary>
/// Reverse in the lane
/// </summary>
/// <param name="lane"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicleSpeed"></param>
/// <returns></returns>
private StayInLaneBehavior LaneReverseRecovery(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed, TrajectoryBlockedReport blockage)
{
// distance to reverse
double distToReverse = double.IsNaN(blockage.DistanceToBlockage) ? TahoeParams.VL * 2.0 : TahoeParams.VL - blockage.DistanceToBlockage;
if (distToReverse <= 3.0)
distToReverse = TahoeParams.VL;
// just reverse and let brian catch it
StayInLaneBehavior reverseBehavior = new StayInLaneBehavior(
lane.LaneId, new StopAtDistSpeedCommand(distToReverse, true),
new List<int>(), lane.LanePath(), lane.Width,
lane.NumberOfLanesLeft(vehicleState.Front, true), lane.NumberOfLanesRight(vehicleState.Front, true));
#region Start too close to start of lane
// check distance to the start of the lane
double laneStartDistanceFromFront = lane.DistanceBetween(lane.WaypointList[0].Position, vehicleState.Front);
if (laneStartDistanceFromFront < TahoeParams.VL)
{
// make sure we're not at the very start of the lane
if (laneStartDistanceFromFront < 0.5)
{
// do nothing
return null;
}
// otherwise back up to the start of the lane
else
{
// output
ArbiterOutput.Output("Too close to the start of the lane, setting reverse distance to TP.VL");
// set proper distance
distToReverse = TahoeParams.VL;
// set behavior speed (no car behind us as too close to start of lane)
LinePath bp = vehicleState.VehicleLinePath;
reverseBehavior.SpeedCommand = new StopAtDistSpeedCommand(distToReverse, true);
StayInLaneBehavior silb = new StayInLaneBehavior(null, reverseBehavior.SpeedCommand, new List<int>(), bp, lane.Width * 1.5, 0, 0);
return silb;
}
}
#endregion
#region Sparse
// check distance to the start of the lane
if (lane.GetClosestPartition(vehicleState.Front).Type == PartitionType.Sparse)
{
// set behavior speed (no car behind us as too close to start of lane)
LinePath bp = vehicleState.VehicleLinePath;
reverseBehavior.SpeedCommand = new StopAtDistSpeedCommand(distToReverse, true);
StayInLaneBehavior silb = new StayInLaneBehavior(null, reverseBehavior.SpeedCommand, new List<int>(), bp, lane.Width * 1.5, 0, 0);
return silb;
}
#endregion
#region Vehicle Behind us
// rear monitor
RearQuadrantMonitor rearMonitor = this.tacticalUmbrella.roadTactical.forwardReasoning.RearMonitor;
// update and check for vehicle
rearMonitor.Update(vehicleState);
if (rearMonitor.CurrentVehicle != null)
{
// check for not failed forward vehicle
if (rearMonitor.CurrentVehicle.QueuingState.Queuing != QueuingState.Failed)
{
// check if enough room to rear vehicle
double vehicleDistance = lane.DistanceBetween(rearMonitor.CurrentVehicle.ClosestPosition, vehicleState.Rear);
if (vehicleDistance < distToReverse - 2.0)
{
// revised distance given vehicle
double revisedDistance = vehicleDistance - 2.0;
// check enough room
if (revisedDistance > TahoeParams.VL)
{
// set the updated speed command
reverseBehavior.SpeedCommand = new StopAtDistSpeedCommand(revisedDistance, true);
}
// not enough room
else
{
// output not enough room because of vehicle
ArbiterOutput.Output("Blockage recovery, not enough room in rear because of rear vehicle, waiting for it to clear");
return null;
}
}
}
}
#endregion
// all clear, return reverse maneuver, set cooldown
return reverseBehavior;
}
/// <summary>
/// Reverse because of a blockage
/// </summary>
/// <param name="lane"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicleSpeed"></param>
/// <param name="defcon"></param>
/// <param name="saudi"></param>
/// <param name="laneOpposing"></param>
/// <param name="currentBlockage"></param>
/// <param name="ebs"></param>
/// <returns></returns>
private Maneuver LaneReverseManeuver(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed,
BlockageRecoveryDEFCON defcon, SAUDILevel saudi, bool laneOpposing, ITacticalBlockage currentBlockage, EncounteredBlockageState ebs)
{
// distance to reverse
double distToReverse = TahoeParams.VL * 2.0;
// just reverse and let brian catch it
StayInLaneBehavior reverseBehavior = new StayInLaneBehavior(
lane.LaneId, new StopAtDistSpeedCommand(distToReverse, true),
new List<int>(), lane.LanePath(), lane.Width,
lane.NumberOfLanesLeft(vehicleState.Front, true), lane.NumberOfLanesRight(vehicleState.Front, true));
// get the saudi level
List<BehaviorDecorator> decs = new List<BehaviorDecorator>(TurnDecorators.HazardDecorator.ToArray());
decs.Add(new ShutUpAndDoItDecorator(saudi));
reverseBehavior.Decorators = decs;
// state
IState laneState = new StayInLaneState(lane, CoreCommon.CorePlanningState);
BlockageRecoveryState brs = new BlockageRecoveryState(
reverseBehavior, laneState, laneState, defcon, ebs, BlockageRecoverySTATUS.EXECUTING);
// check enough room in lane to reverse
RearQuadrantMonitor rearMonitor = this.tacticalUmbrella.roadTactical.forwardReasoning.RearMonitor;
if (rearMonitor == null || !rearMonitor.lane.Equals(lane))
this.tacticalUmbrella.roadTactical.forwardReasoning.RearMonitor = new RearQuadrantMonitor(lane, SideObstacleSide.Driver);
#region Start too close to start of lane
// check distance to the start of the lane
double laneStartDistanceFromFront = lane.DistanceBetween(lane.WaypointList[0].Position, vehicleState.Front);
if (laneStartDistanceFromFront < TahoeParams.VL)
{
// make sure we're not at the very start of the lane
if (laneStartDistanceFromFront < 0.5)
{
// output
ArbiterOutput.Output("Too close to the start of the lane, raising defcon");
// go up chain
return new Maneuver(new NullBehavior(),
new EncounteredBlockageState(currentBlockage, laneState, BlockageRecoveryDEFCON.WIDENBOUNDS, saudi),
TurnDecorators.NoDecorators,
vehicleState.Timestamp);
}
// otherwise back up to the start of the lane
else
{
// output
ArbiterOutput.Output("Too close to the start of the lane, setting reverse distance to TP.VL");
// set proper distance
distToReverse = TahoeParams.VL;
// set behavior speed (no car behind us as too close to start of lane)
LinePath bp = vehicleState.VehicleLinePath;
reverseBehavior.SpeedCommand = new StopAtDistSpeedCommand(distToReverse, true);
StayInLaneBehavior silb = new StayInLaneBehavior(null, reverseBehavior.SpeedCommand, new List<int>(), bp, lane.Width * 1.5, 0, 0);
return new Maneuver(silb, brs, decs, vehicleState.Timestamp);
}
}
#endregion
#region Sparse
// check distance to the start of the lane
if (lane.GetClosestPartition(vehicleState.Front).Type == PartitionType.Sparse)
{
// set behavior speed (no car behind us as too close to start of lane)
LinePath bp = vehicleState.VehicleLinePath;
reverseBehavior.SpeedCommand = new StopAtDistSpeedCommand(distToReverse, true);
StayInLaneBehavior silb = new StayInLaneBehavior(null, reverseBehavior.SpeedCommand, new List<int>(), bp, lane.Width * 1.5, 0, 0);
return new Maneuver(silb, brs, decs, vehicleState.Timestamp);
}
#endregion
#region Vehicle Behind us
// update and check for vehicle
rearMonitor.Update(vehicleState);
if (rearMonitor.CurrentVehicle != null)
{
// check for not failed forward vehicle
if (rearMonitor.CurrentVehicle.QueuingState.Queuing != QueuingState.Failed)
{
// check if enough room to rear vehicle
double vehicleDistance = lane.DistanceBetween(rearMonitor.CurrentVehicle.ClosestPosition, vehicleState.Rear);
if (vehicleDistance < distToReverse - 2.0)
{
// revised distance given vehicle
double revisedDistance = vehicleDistance - 2.0;
// check enough room
if (revisedDistance > TahoeParams.VL)
{
// set the updated speed command
reverseBehavior.SpeedCommand = new StopAtDistSpeedCommand(revisedDistance, true);
}
// not enough room
else
{
// output not enough room because of vehicle
ArbiterOutput.Output("Blockage recovery, not enough room in rear because of rear vehicle, waiting for it to clear");
return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
}
// check failed rear vehicle, up defcon
else
{
// failed vehicle in rear, go up chain
return new Maneuver(new NullBehavior(),
new EncounteredBlockageState(currentBlockage, laneState, BlockageRecoveryDEFCON.WIDENBOUNDS, saudi),
TurnDecorators.NoDecorators,
vehicleState.Timestamp);
}
}
#endregion
// all clear, return reverse maneuver, set cooldown
BlockageHandler.SetDefaultBlockageCooldown();
return new Maneuver(reverseBehavior, brs, decs, vehicleState.Timestamp);
}
/// <summary>
/// Determine a change lanes recovery behavior
/// </summary>
/// <param name="current"></param>
/// <param name="vs"></param>
/// <param name="shouldWait"></param>
/// <returns></returns>
private ChangeLaneBehavior ChangeLanesRecoveryBehavior(ArbiterLane current, VehicleState vs, out bool shouldWait, out IState completionState)
{
// set defaults
shouldWait = false;
completionState = null;
// check partition type to make sure normal
if(current.GetClosestPartition(vs.Front).Type != PartitionType.Normal)
return null;
// make sure not in a safety zone
foreach(ArbiterSafetyZone asz in current.SafetyZones)
{
if(asz.IsInSafety(vs.Front))
return null;
}
// check not inside an intersection safety zone
foreach(ArbiterIntersection aInter in current.Way.Segment.RoadNetwork.ArbiterIntersections.Values)
{
if(aInter.IntersectionPolygon.IsInside(vs.Front) && (aInter.StoppedExits != null && aInter.StoppedExits.Count > 0))
return null;
}
// get the distance to the next lane major
ArbiterWaypoint nextWaypoint = current.GetClosestPartition(vs.Front).Final;
List<WaypointType> majorLaneTypes = new List<WaypointType>(new WaypointType[]{ WaypointType.End, WaypointType.Stop});
double distanceToNextMajor = current.DistanceBetween(vs.Front, current.GetNext(nextWaypoint, majorLaneTypes, new List<ArbiterWaypoint>()).Position);
// check distance > 3.0
if(distanceToNextMajor > 30.0)
{
// check clear on right, or if it does not exist here
ILateralReasoning rightLateral = this.tacticalUmbrella.roadTactical.rightLateralReasoning;
bool useRight = rightLateral.Exists && rightLateral.ExistsExactlyHere(vs) && rightLateral is LateralReasoning;
// check clear on left, or if it does not exist here
ILateralReasoning leftLateral = this.tacticalUmbrella.roadTactical.leftLateralReasoning;
bool useLeft = leftLateral.Exists && leftLateral.ExistsExactlyHere(vs) && leftLateral is LateralReasoning;
// notify
ArbiterOutput.Output("Blockage recovery: lane change left ok to use: " + useLeft.ToString() + ", use righrt: " + useRight.ToString());
#region Test Right
// determine if should use, should wait, or should not use
LaneChangeMonitorResult rightLCMR = LaneChangeMonitorResult.DontUse;
// check usability of the right lateral maneuver
if(useRight && rightLateral is LateralReasoning)
{
// check adj and rear clear
bool adjRearClear = rightLateral.AdjacentAndRearClear(vs);
// wait maybe if not clear
if(!adjRearClear && (rightLateral.AdjacentVehicle == null || rightLateral.AdjacentVehicle.QueuingState.Queuing != QueuingState.Failed))
rightLCMR = LaneChangeMonitorResult.Wait;
else if(adjRearClear)
rightLCMR = LaneChangeMonitorResult.Use;
else
rightLCMR = LaneChangeMonitorResult.DontUse;
// check down
if (rightLCMR == LaneChangeMonitorResult.Wait)
{
ArbiterOutput.Output("Blockage lane changes waiting for right lateral lane to clear");
shouldWait = true;
return null;
}
// check use
else if (rightLCMR == LaneChangeMonitorResult.Use)
{
// get the behavior
ChangeLaneBehavior rightClb = new ChangeLaneBehavior(current.LaneId, rightLateral.LateralLane.LaneId, false,
TahoeParams.VL * 1.5, new ScalarSpeedCommand(2.24), new List<int>(), current.LanePath(), rightLateral.LateralLane.LanePath(),
current.Width, rightLateral.LateralLane.Width, current.NumberOfLanesLeft(vs.Front, true), current.NumberOfLanesRight(vs.Front, true));
// test
CompletionReport rightClbCr;
bool successCL = CoreCommon.Communications.TestExecute(rightClb, out rightClbCr);
if (successCL && rightClbCr is SuccessCompletionReport)
{
ArbiterOutput.Output("Blockage lane changes found good behavior to right lateral lane");
shouldWait = false;
completionState = new StayInLaneState(rightLateral.LateralLane, CoreCommon.CorePlanningState);
return rightClb;
}
}
}
#endregion
#region Test Left Forwards
if(useLeft && leftLateral is LateralReasoning)
{
// determine if should use, should wait, or should not use
LaneChangeMonitorResult leftLCMR = LaneChangeMonitorResult.DontUse;
// check usability of the left lateral maneuver
if (useLeft && leftLateral is LateralReasoning)
{
// check adj and rear clear
bool adjRearClear = leftLateral.AdjacentAndRearClear(vs);
// wait maybe if not clear
if (!adjRearClear && (leftLateral.AdjacentVehicle == null || leftLateral.AdjacentVehicle.QueuingState.Queuing != QueuingState.Failed))
leftLCMR = LaneChangeMonitorResult.Wait;
else if (adjRearClear)
leftLCMR = LaneChangeMonitorResult.Use;
else
leftLCMR = LaneChangeMonitorResult.DontUse;
ArbiterOutput.Output("Blockage recovery, lane change left: " + leftLCMR.ToString());
// check down
if (leftLCMR == LaneChangeMonitorResult.Wait)
{
ArbiterOutput.Output("Blockage recovery, Blockage lane changes waiting for left lateral lane to clear");
shouldWait = true;
return null;
}
// check use
else if (leftLCMR == LaneChangeMonitorResult.Use)
{
// get the behavior
ChangeLaneBehavior leftClb = new ChangeLaneBehavior(current.LaneId, leftLateral.LateralLane.LaneId, false,
TahoeParams.VL * 1.5, new ScalarSpeedCommand(2.24), new List<int>(), current.LanePath(), leftLateral.LateralLane.LanePath(),
current.Width, leftLateral.LateralLane.Width, current.NumberOfLanesLeft(vs.Front, true), current.NumberOfLanesLeft(vs.Front, true));
// test
CompletionReport leftClbCr;
bool successCL = CoreCommon.Communications.TestExecute(leftClb, out leftClbCr);
if (successCL && leftClbCr is SuccessCompletionReport)
{
ArbiterOutput.Output("Blockage recovery, Blockage lane changes found good behavior to left lateral lane");
completionState = new StayInLaneState(leftLateral.LateralLane, CoreCommon.CorePlanningState);
shouldWait = false;
return leftClb;
}
}
}
}
#endregion
}
// fallout return null
return null;
}
/// <summary>
/// Determines proper speed commands given we want to stop at a certain waypoint
/// </summary>
/// <param name="waypoint"></param>
/// <param name="lane"></param>
/// <param name="position"></param>
/// <param name="enCovariance"></param>
/// <param name="stopSpeed"></param>
/// <param name="stopDistance"></param>
public void StoppingParams(ArbiterWaypoint waypoint, ArbiterLane lane, Coordinates position, double extraDistance,
out double stopSpeed, out double stopDistance)
{
// get dist to waypoint
stopDistance = lane.DistanceBetween(position, waypoint.Position) - extraDistance;
// speed tools
stopSpeed = SpeedTools.GenerateSpeed(stopDistance, CoreCommon.OperationalStopSpeed, 2.24);
}
/// <summary>
/// Gets the next navigational stop relavant to us (stop or end) in the closest good lane or our current opposing lane
/// </summary>
/// <param name="closestGood"></param>
/// <param name="coordinates"></param>
/// <param name="ignorable"></param>
/// <param name="navStopSpeed"></param>
/// <param name="navStopDistance"></param>
/// <param name="navStopType"></param>
/// <param name="navStop"></param>
private void NextOpposingNavigationalStop(ArbiterLane opposing, ArbiterLane closestGood, Coordinates coordinates, double extraDistance,
out double navStopSpeed, out double navStopDistance, out StopType navStopType, out ArbiterWaypoint navStop)
{
ArbiterWaypoint current = null;
double minDist = Double.MaxValue;
StopType st = StopType.EndOfLane;
#region Closest Good Parameterization
foreach (ArbiterWaypoint aw in closestGood.WaypointList)
{
if (aw.IsStop || aw.NextPartition == null)
{
double dist = closestGood.DistanceBetween(coordinates, aw.Position);
if (dist < minDist && dist >= 0)
{
current = aw;
minDist = dist;
st = aw.IsStop ? StopType.StopLine : StopType.EndOfLane;
}
}
}
#endregion
#region Opposing Parameterization
ArbiterWaypoint opStart = opposing.GetClosestPartition(coordinates).Initial;
int startIndex = opposing.WaypointList.IndexOf(opStart);
for (int i = startIndex; i >= 0; i--)
{
ArbiterWaypoint aw = opposing.WaypointList[i];
if (aw.IsStop || aw.PreviousPartition == null)
{
double dist = opposing.DistanceBetween(aw.Position, coordinates);
if (dist < minDist && dist >= 0)
{
current = aw;
minDist = dist;
st = aw.IsStop ? StopType.StopLine : StopType.EndOfLane;
}
}
}
#endregion
double tmpDistanceIgnore;
this.StoppingParams(current, closestGood, coordinates, extraDistance, out navStopSpeed, out tmpDistanceIgnore);
navStop = current;
navStopDistance = minDist;
navStopType = st;
}
/// <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>
/// Secondary maneuver outside minimum cap
/// </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? AdvancedSecondaryOutsideMinCap(ArbiterLane lane, ArbiterWaypoint laneGoal, VehicleState vehicleState, RoadPlan roadPlan,
List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, TypeOfTasks bestTask)
{
// get proper reasoning component
List<LateralReasoning> adjacentReasonings = new List<LateralReasoning>();
if (bestTask != TypeOfTasks.Left)
{
if (this.rightLateralReasoning is LateralReasoning && this.rightLateralReasoning.Exists && this.rightLateralReasoning.ExistsExactlyHere(vehicleState))
adjacentReasonings.Add((LateralReasoning)this.rightLateralReasoning);
else if (this.leftLateralReasoning is LateralReasoning && this.leftLateralReasoning.Exists && this.leftLateralReasoning.ExistsExactlyHere(vehicleState))
adjacentReasonings.Add((LateralReasoning)this.leftLateralReasoning);
}
else
{
if (this.leftLateralReasoning is LateralReasoning && this.leftLateralReasoning.Exists && this.leftLateralReasoning.ExistsExactlyHere(vehicleState))
adjacentReasonings.Add((LateralReasoning)this.leftLateralReasoning);
else if (this.rightLateralReasoning is LateralReasoning && this.rightLateralReasoning.Exists && this.rightLateralReasoning.ExistsExactlyHere(vehicleState))
adjacentReasonings.Add((LateralReasoning)this.rightLateralReasoning);
}
// loop through possible
foreach(LateralReasoning adjacentReasoning in adjacentReasonings)
{
// check if adjacent reasoning exists
if (adjacentReasoning != null && adjacentReasoning.Exists)
{
// update adjacent reasoning
adjacentReasoning.ForwardMonitor.Primary(adjacentReasoning.LateralLane, vehicleState, roadPlan, blockages, ignorable, false);
// otherwise check if forward vehicle slow
LaneChangeInformation forwardVehicleSecondary;
// check if should pass the forward vehicle
if (this.ForwardMonitor.ForwardVehicle.ShouldPass(out forwardVehicleSecondary, lane))
{
// check if forward vehicle failed
if (forwardVehicleSecondary.Reason == LaneChangeReason.FailedForwardVehicle)
{
ArbiterOutput.WriteToLog("AdvancedSecondaryOutsideMinCap: Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " failed");
// make sure the failed vehicle is not within 50m of the goal
double vehicleDistanceToGoal = lane.DistanceBetween(ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition, laneGoal.Position);
ArbiterOutput.Output("Failed FV: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + ", DistGoal: " + vehicleDistanceToGoal.ToString("f2") + ", speed: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed.ToString("f1"));
if ((vehicleDistanceToGoal > 50))
{
// check if adjacent has no forward vehicle
if (!adjacentReasoning.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker)
{
// plan the lane change
return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, forwardVehicleSecondary);
}
// otherwise a forward vehicle exists
else
{
// otherwise check if forward vehicle slow
LaneChangeInformation lateralVehicleInformation;
// check if lateral vehicle fine
if (!adjacentReasoning.ForwardMonitor.ForwardVehicle.ShouldPass(out lateralVehicleInformation, adjacentReasoning.LateralLane))
{
// plan the lane change
return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, new LaneChangeInformation(LaneChangeReason.Navigation, adjacentReasoning.LateralMonitor.CurrentVehicle));
}
// check if lateral vehicle failed or slow
else
{
// check distance to lateral > distance to forward + 25
double distToAdjacent = lane.DistanceBetween(vehicleState.Front, adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);
double distToForward = lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);
if (distToAdjacent > distToForward + 25.0)
{
// plan the lane change
return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, new LaneChangeInformation(LaneChangeReason.Navigation, adjacentReasoning.LateralMonitor.CurrentVehicle));
}
}
}
}
}
// otherwise check if they are slow
else if (forwardVehicleSecondary.Reason == LaneChangeReason.SlowForwardVehicle)
{
ArbiterOutput.WriteToLog("AdvancedSecondaryOutsideMinCap: Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " slow");
// make sure the slow vehicle is not within 50m of the goal if velocity is > 5mph
double vehicleDistanceToGoal = lane.DistanceBetween(ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition, laneGoal.Position);
ArbiterOutput.Output("Slow FV: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + ", DistGoal: " + vehicleDistanceToGoal.ToString("f2") + ", speed: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed.ToString("f1"));
if ((vehicleDistanceToGoal > 50 && this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed < 2.24) ||
(vehicleDistanceToGoal > 75 && this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed < 4.48) ||
(vehicleDistanceToGoal > 100 && this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed < 6.72) ||
(vehicleDistanceToGoal > 125 && this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed < 8.96))
{
// check if adjacent has no forward vehicle
if (!adjacentReasoning.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker)
{
// plan the lane change
ArbiterOutput.WriteToLog("AdvancedSecondaryOutsideMinCap: No vehicle in adjacent");
return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, new LaneChangeInformation(LaneChangeReason.Navigation, adjacentReasoning.LateralMonitor.CurrentVehicle));
}
// otherwise a forward vehicle exists
else
{
// otherwise check if forward vehicle slow
LaneChangeInformation lateralVehicleInformation;
// check if lateral vehicle fine
if (!adjacentReasoning.ForwardMonitor.ForwardVehicle.ShouldPass(out lateralVehicleInformation, adjacentReasoning.LateralLane))
{
// check distance to lateral > distance to forward + 25
double distToAdjacent = lane.DistanceBetween(vehicleState.Front, adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);
double distToForward = lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);
ArbiterOutput.WriteToLog("AdvancedSecondaryOutsideMinCap: Normal vehicle in adjacent: " + adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + "FV Dist: " + distToForward.ToString("f2") + ", Adj Dist: " + adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString());
// check distance greater and adjacent speed greater than forward speed
if (distToAdjacent > distToForward + 25.0 &&
adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed > this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed)
{
// plan the lane change
return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, new LaneChangeInformation(LaneChangeReason.Navigation, adjacentReasoning.LateralMonitor.CurrentVehicle));
}
// plan the lane change
//return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, new LaneChangeInformation(LaneChangeReason.Navigation, adjacentReasoning.LateralMonitor.CurrentVehicle));
}
else if (
lane.DistanceBetween(this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition,
adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) > 65.0 &&
(this.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped ||
(this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed + 4.48 < adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed)))
{
// plan the lane change
return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, new LaneChangeInformation(LaneChangeReason.Navigation, adjacentReasoning.LateralMonitor.CurrentVehicle));
}
}
}
}
}
}
}
// normal secondary parameterization as a fall through on the naviagation lane changes
return this.SecondaryManeuver(lane, vehicleState, roadPlan, blockages, ignorable, bestTask);
}
/// <summary>
/// Secondary maneuver when current lane is the desired lane
/// </summary>
/// <param name="arbiterLane"></param>
/// <param name="vehicleState"></param>
/// <param name="roadPlan"></param>
/// <param name="blockages"></param>
/// <param name="ignorable"></param>
/// <returns></returns>
public Maneuver? AdvancedSecondaryNextCheck(ArbiterLane lane, ArbiterWaypoint laneGoal, VehicleState vehicleState, RoadPlan roadPlan,
List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, TypeOfTasks bestTask)
{
// check if the forward vehicle exists
if (this.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker && this.ForwardMonitor.ForwardVehicle.CurrentVehicle.PassedLongDelayedBirth)
{
// distance to forward vehicle
double distanceToForwardVehicle = lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);
#region Distance From Forward Not Enough
// check distance to forward vehicle
if (this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.Queuing == QueuingState.Failed &&
distanceToForwardVehicle < 8.0 && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle)
{
// set name
ArbiterLane al = lane;
// distance to forward vehicle too small
double distToForwards = al.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);
double distToReverse = Math.Max(1.0, 8.0 - distToForwards);
if (distToForwards < 8.0)
{
// notify
ArbiterOutput.Output("Secondary: NOT Properly Stopped Behind Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " distance: " + distToForwards.ToString("f2"));
this.RearMonitor = new RearQuadrantMonitor(al, SideObstacleSide.Driver);
this.RearMonitor.Update(vehicleState);
if (this.RearMonitor.CurrentVehicle != null)
{
double distToRearVehicle = al.DistanceBetween(this.RearMonitor.CurrentVehicle.ClosestPosition, vehicleState.Position) - TahoeParams.RL;
double distNeedClear = distToReverse + 2.0;
if (distToRearVehicle < distNeedClear)
{
// notify
ArbiterOutput.Output("Secondary: Rear: Not enough room to clear in rear: " + distToRearVehicle.ToString("f2") + " < " + distNeedClear.ToString("f2"));
return null;
}
}
double distToLaneStart = al.DistanceBetween(al.LanePath().StartPoint.Location, vehicleState.Position) - TahoeParams.RL;
if (distToReverse > distToLaneStart)
{
// notify
ArbiterOutput.Output("Secondary: Rear: Not enough room in lane to reverse in rear: " + distToLaneStart.ToString("f2") + " < " + distToReverse.ToString("f2"));
return null;
}
else
{
// notify
ArbiterOutput.Output("Secondary: Reversing to pass Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " reversing distance: " + distToReverse.ToString("f2"));
StopAtDistSpeedCommand sadsc = new StopAtDistSpeedCommand(distToReverse, true);
StayInLaneBehavior silb = new StayInLaneBehavior(al.LaneId, sadsc, new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(vehicleState.Front, true), al.NumberOfLanesRight(vehicleState.Front, true));
return new Maneuver(silb, CoreCommon.CorePlanningState, TurnDecorators.HazardDecorator, vehicleState.Timestamp);
}
}
}
#endregion
// get distance to next lane major
List<WaypointType> wts = new List<WaypointType>(new WaypointType[] { WaypointType.Stop, WaypointType.End });
ArbiterWaypoint nextWaypoint = lane.GetNext(lane.GetClosestPartition(vehicleState.Position).Final, wts, new List<ArbiterWaypoint>());
// check if the vehicle occurs before the next major thing
double distanceToNextMajor = lane.DistanceBetween(vehicleState.Front, nextWaypoint.Position);
// check if vehicle occurs before the next lane major
if (distanceToForwardVehicle < distanceToNextMajor)
{
// check if forward vehicle exists in this lane and is closer then the lane goal
if (TacticalDirector.VehicleAreas.ContainsKey(lane) &&
TacticalDirector.VehicleAreas[lane].Contains(this.ForwardMonitor.ForwardVehicle.CurrentVehicle) &&
lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) < lane.DistanceBetween(vehicleState.Front, laneGoal.Position))
{
// some constants
double desiredDistance = 50.0;
// get the lane goal distance
double distanceToLaneGoal = lane.DistanceBetween(vehicleState.Front, laneGoal.Position);
// check if necessary to be in current lane
if (distanceToLaneGoal <= desiredDistance)
{
// default secondary maneuver
ArbiterOutput.WriteToLog("AdvancedSecondaryNextCheck: DistToLaneGoal < 50: " + desiredDistance.ToString("f2"));
return this.SecondaryManeuver(lane, vehicleState, roadPlan, new List<ITacticalBlockage>(), new List<ArbiterWaypoint>(), bestTask);
}
// otherwise would be good but not necessary
else
{
// return the maneuver from the vehicle being outside the min cap
ArbiterOutput.WriteToLog("AdvancedSecondaryNextCheck: DistToLaneGoal > 50: " + desiredDistance.ToString("f2"));
return this.AdvancedSecondaryOutsideMinCap(lane, laneGoal, vehicleState, roadPlan, blockages, ignorable, bestTask);
}
}
}
}
// no secondary
return null;
}
/// <summary>
/// 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>
/// Updates the current vehicle
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
public void Update(ArbiterLane lane, VehicleState state)
{
// get the forward path
LinePath p = lane.LanePath().Clone();
p.Reverse();
// get our position
Coordinates f = state.Front;
// get all vehicles associated with those components
List<VehicleAgent> vas = new List<VehicleAgent>();
foreach (IVehicleArea iva in lane.AreaComponents)
{
if (TacticalDirector.VehicleAreas.ContainsKey(iva))
vas.AddRange(TacticalDirector.VehicleAreas[iva]);
}
// get the closest forward of us
double minDistance = Double.MaxValue;
VehicleAgent closest = null;
// get clsoest
foreach (VehicleAgent va in vas)
{
// get position of front
Coordinates frontPos = va.ClosestPosition;
// gets distance from other vehicle to us along the lane
double frontDist = lane.DistanceBetween(frontPos, f);
if (frontDist >= 0 && frontDist < minDistance)
{
minDistance = frontDist;
closest = va;
}
}
this.CurrentVehicle = closest;
this.currentDistance = minDistance;
}
/// <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;
}
}