/// <summary>
/// checks if a pointon path is in a safety zone
/// </summary>
/// <param name="pop"></param>
/// <returns></returns>
public bool IsInSafety(LinePath.PointOnPath pop)
{
double distIn = lane.LanePath().DistanceBetween(safetyZoneBegin, pop);
if (distIn < 30 && distIn >= 0)
{
return(true);
}
else
{
return(false);
}
}
/// <summary>
/// Gets parameterization
/// </summary>
/// <param name="lane"></param>
/// <param name="speed"></param>
/// <param name="distance"></param>
/// <param name="state"></param>
/// <returns></returns>
public TravelingParameters GetParameters(ArbiterLane lane, double speed, double distance, VehicleState state)
{
double distanceCutOff = CoreCommon.OperationslStopLineSearchDistance;
Maneuver m = new Maneuver();
SpeedCommand sc;
bool usingSpeed = true;
#region Distance Cutoff
// check if distance is less than cutoff
if (distance < distanceCutOff)
{
// default behavior
sc = new StopAtDistSpeedCommand(distance);
Behavior b = new StayInLaneBehavior(lane.LaneId, sc, new List<int>(), lane.LanePath(), lane.Width, lane.NumberOfLanesLeft(state.Front, true), lane.NumberOfLanesRight(state.Front, true));
// stopping so not using speed param
usingSpeed = false;
// standard behavior is fine for maneuver
m = new Maneuver(b, new StayInLaneState(lane, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, state.Timestamp);
}
#endregion
#region Outisde Distance Envelope
// not inside distance envalope
else
{
// get lane
ArbiterLane al = lane;
// default behavior
sc = new ScalarSpeedCommand(speed);
Behavior b = new StayInLaneBehavior(al.LaneId, sc, new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true));
// standard behavior is fine for maneuver
m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, state.Timestamp);
}
#endregion
#region Parameterize
// create new params
TravelingParameters tp = new TravelingParameters();
tp.Behavior = m.PrimaryBehavior;
tp.Decorators = m.PrimaryBehavior.Decorators;
tp.DistanceToGo = distance;
tp.NextState = m.PrimaryState;
tp.RecommendedSpeed = speed;
tp.Type = TravellingType.Navigation;
tp.UsingSpeed = usingSpeed;
tp.SpeedCommand = sc;
tp.VehiclesToIgnore = new List<int>();
// return navigation params
return tp;
#endregion
}
/// <summary>
/// Maneuver for recovering from a lane blockage, used for lane changes as well
/// </summary>
/// <param name="lane"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicleSpeed"></param>
/// <param name="defcon"></param>
/// <param name="saudi"></param>
/// <returns></returns>
public Maneuver OpposingLaneRecoveryManeuver(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed,
INavigationalPlan plan, BlockageRecoveryState brs, bool failedVehiclesPersistentlyOnly)
{
// get the blockage
ITacticalBlockage laneBlockageReport = brs.EncounteredState.TacticalBlockage;
// get the turn state
OpposingLanesState sils = (OpposingLanesState)brs.EncounteredState.PlanningState;
#region Reverse
// check the state of the recovery for being the initial state
if (brs.Defcon == BlockageRecoveryDEFCON.INITIAL)
{
// determine if the reverse behavior is recommended
if (laneBlockageReport.BlockageReport.ReverseRecommended)
{
// notify
ArbiterOutput.Output("Initial encounter, reverse recommended, reversing");
// path
LinePath revPath = lane.LanePath().Clone();
revPath.Reverse();
// dist to reverse
double distToReverse;
if (double.IsNaN(laneBlockageReport.BlockageReport.DistanceToBlockage))
distToReverse = TahoeParams.VL * 1.5;
else if (laneBlockageReport.BlockageReport.DistanceToBlockage < 0 || laneBlockageReport.BlockageReport.DistanceToBlockage > 2 * TahoeParams.VL)
distToReverse = TahoeParams.VL * 1.5;
else
distToReverse = TahoeParams.VL - laneBlockageReport.BlockageReport.DistanceToBlockage;
// get reverse behavior
StayInLaneBehavior reverseRecovery = new StayInLaneBehavior(lane.LaneId,
new StopAtDistSpeedCommand(TahoeParams.VL * 2.0, true), new List<int>(),
revPath, lane.Width, lane.NumberOfLanesLeft(vehicleState.Front, false), lane.NumberOfLanesRight(vehicleState.Front, false));
// get recovery state
BlockageRecoveryState brsT = new BlockageRecoveryState(
reverseRecovery, CoreCommon.CorePlanningState, new OpposingLanesState(lane, false, CoreCommon.CorePlanningState, vehicleState),
brs.Defcon = BlockageRecoveryDEFCON.REVERSE, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
brsT.CompletionState = brsT;
// reset blockages
BlockageHandler.SetDefaultBlockageCooldown();
// maneuver
return new Maneuver(reverseRecovery, brsT, TurnDecorators.HazardDecorator, vehicleState.Timestamp);
}
}
#endregion
#region Recovery Escalation
// plan forward reasoning
this.tacticalUmbrella.roadTactical.opposingReasoning.ForwardManeuver(lane,
((OpposingLanesState)brs.EncounteredState.PlanningState).ClosestGoodLane,
vehicleState, (RoadPlan)plan, new List<ITacticalBlockage>());
// check clear on right, or if it does nto exist here
ILateralReasoning rightLateral = this.tacticalUmbrella.roadTactical.rightLateralReasoning;
bool rightClear = !rightLateral.Exists || !rightLateral.ExistsExactlyHere(vehicleState) ||
rightLateral.AdjacentAndRearClear(vehicleState);
// check clear on left, or if it does nto exist here
ILateralReasoning leftLateral = this.tacticalUmbrella.roadTactical.leftLateralReasoning;
bool leftClear = !leftLateral.Exists || !leftLateral.ExistsExactlyHere(vehicleState) ||
leftLateral.AdjacentAndRearClear(vehicleState);
if (leftClear && leftLateral is OpposingLateralReasoning)
leftClear = leftLateral.ForwardClear(vehicleState, TahoeParams.VL * 3.0, 2.24,
new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, null),
lane.LanePath().AdvancePoint(lane.LanePath().GetClosestPoint(vehicleState.Front), TahoeParams.VL * 3.0).Location);
// check both clear to widen
bool widenOk = rightClear && leftClear;
// Notify
ArbiterOutput.Output("Blockage tactical recovery escalation: rightClear: " + rightClear.ToString() + ", leftClear: " + leftClear.ToString());
// path
LinePath revPath2 = lane.LanePath().Clone();
revPath2.Reverse();
// check widen
if (widenOk)
{
// output
ArbiterOutput.Output("Lane Blockage Recovery: Adjacent areas clear");
if (brs.Defcon != BlockageRecoveryDEFCON.CHANGELANES_FORWARD)
{
// check change lanes back to good
ChangeLaneBehavior clb = new ChangeLaneBehavior(
lane.LaneId, sils.ClosestGoodLane.LaneId, false, TahoeParams.VL * 3.0, new StopAtDistSpeedCommand(TahoeParams.VL * 3.0), new List<int>(),
revPath2, sils.ClosestGoodLane.LanePath(), lane.Width, sils.ClosestGoodLane.Width,
lane.NumberOfLanesLeft(vehicleState.Front, false), lane.NumberOfLanesRight(vehicleState.Front, false));
// change lanes
brs.Defcon = BlockageRecoveryDEFCON.CHANGELANES_FORWARD;
// cehck ok
CompletionReport clROk;
bool clCheck = CoreCommon.Communications.TestExecute(clb, out clROk);
// check change lanes ok
if (clCheck)
{
ArbiterOutput.Output("Change lanes behaviro ok, executing");
// return manevuer
return new Maneuver(clb, brs, TurnDecorators.RightTurnDecorator, vehicleState.Timestamp);
}
else
{
ArbiterOutput.Output("Change lanes behaviro not ok");
}
}
// if we can't widen for some reason just go through with no widen
StayInLaneBehavior silb = new StayInLaneBehavior(lane.LaneId,
new StopAtDistSpeedCommand(TahoeParams.VL * 2.0), new List<int>(),
revPath2,
lane.Width, lane.NumberOfLanesLeft(vehicleState.Front, false), lane.NumberOfLanesRight(vehicleState.Front, false));
// mini icrease width
silb.LaneWidth = silb.LaneWidth + TahoeParams.T;
// check execute none saudi
CompletionReport l0Cr;
bool l0TestOk = CoreCommon.Communications.TestExecute(silb, out l0Cr);
// check mini ok
if (l0TestOk)
{
// notify
ArbiterOutput.Output("Lane Blockage Recovery: Test Tahoe.T lane widen ok");
// update the current state
BlockageRecoveryState brsL0 = new BlockageRecoveryState(
silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
return new Maneuver(silb, brsL0, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
{
// notify
ArbiterOutput.Output("Lane Blockage Recovery: Test Tahoe.T lane widen failed, moving to large widen");
// increase width
silb.LaneWidth = silb.LaneWidth * 3.0;
// check execute l1
CompletionReport l1Cr;
bool l1TestOk = CoreCommon.Communications.TestExecute(silb, out l1Cr);
// check ok
if (l1TestOk)
{
// notify
ArbiterOutput.Output("Lane Blockage Recovery: Test 3LW lane large widen ok");
// update the current state
BlockageRecoveryState brsL1 = new BlockageRecoveryState(
silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
return new Maneuver(silb, brsL1, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// go to l2 for all the marbles
else
{
// notify
ArbiterOutput.Output("Lane Blockage Recovery: Test 3LW lane large widen failed, moving to 3LW, L1 Saudi");
ShutUpAndDoItDecorator saudi2 = new ShutUpAndDoItDecorator(SAUDILevel.L1);
List<BehaviorDecorator> saudi2bds = new List<BehaviorDecorator>(new BehaviorDecorator[] { saudi2 });
silb.Decorators = saudi2bds;
BlockageRecoveryState brsL2 = new BlockageRecoveryState(
silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
// check execute l1
CompletionReport l2Cr;
bool l2TestOk = CoreCommon.Communications.TestExecute(silb, out l1Cr);
// go
return new Maneuver(silb, brsL2, saudi2bds, vehicleState.Timestamp);
}
}
}
#endregion
// fallout goes back to lane state
return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
/// <summary>
/// Maneuver for recovering from a 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>
/// Get the default recovery behavior
/// </summary>
/// <param name="lane"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicleSpeed"></param>
/// <param name="brs"></param>
/// <param name="ebs"></param>
/// <returns></returns>
private StayInLaneBehavior LaneRecoveryBehavior(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed, INavigationalPlan plan,
BlockageRecoveryState brs, EncounteredBlockageState ebs)
{
#region Get the Recovery Behavior
// set the default distance to go forwards
double distanceForwards = TahoeParams.VL * 3.0;
// check distance to next lane point
this.tacticalUmbrella.roadTactical.forwardReasoning.ForwardMonitor.Primary(lane, vehicleState, (RoadPlan)plan, new List<ITacticalBlockage>() , new List<ArbiterWaypoint>(), false);
// get navigation distance to go
double navDistanceToGo = this.tacticalUmbrella.roadTactical.forwardReasoning.ForwardMonitor.NavigationParameters.DistanceToGo;
distanceForwards = navDistanceToGo < distanceForwards ? navDistanceToGo : distanceForwards;
// check if there is a forward vehicle we should follow normally
if (this.tacticalUmbrella.roadTactical.forwardReasoning.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker)
{
// fv distance
double fvDistance = this.tacticalUmbrella.roadTactical.forwardReasoning.ForwardMonitor.ForwardVehicle.ForwardControl.xSeparation;
// check distance forwards to vehicle
if (fvDistance < distanceForwards)
{
// distance modification
distanceForwards = fvDistance > 2.0 ? fvDistance - 2.0 : fvDistance;
}
}
// test behavior
StayInLaneBehavior testForwards = new StayInLaneBehavior(
lane.LaneId, new StopAtDistSpeedCommand(distanceForwards), new List<int>(), lane.LanePath(), lane.Width,
lane.NumberOfLanesLeft(vehicleState.Front, true), lane.NumberOfLanesRight(vehicleState.Front, true));
// set the specifiec saudi level
testForwards.Decorators = new List<BehaviorDecorator>(new BehaviorDecorator[] { new ShutUpAndDoItDecorator(brs.EncounteredState.Saudi) });
// return the test
return testForwards;
#endregion
}
/// <summary>
/// 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>
/// 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>
/// Generate the traveling parameterization for the desired behaivor
/// </summary>
/// <param name="lane"></param>
/// <param name="navStopSpeed"></param>
/// <param name="navStopDistance"></param>
/// <param name="navStop"></param>
/// <param name="navStopType"></param>
/// <param name="state"></param>
/// <returns></returns>
private TravelingParameters NavStopParameterization(ArbiterLane lane, double navStopSpeed, double navStopDistance, ArbiterWaypoint navStop, StopType navStopType, VehicleState state)
{
// get min dist
double distanceCutOff = CoreCommon.OperationalStopDistance;
// turn direction default
List<BehaviorDecorator> decorators = TurnDecorators.NoDecorators;
// create new params
TravelingParameters tp = new TravelingParameters();
#region Get Maneuver
Maneuver m = new Maneuver();
bool usingSpeed = true;
// get lane path
LinePath lp = lane.LanePath().Clone();
lp.Reverse();
#region Distance Cutoff
// check if distance is less than cutoff
if (navStopDistance < distanceCutOff)
{
// default behavior
tp.SpeedCommand = new StopAtDistSpeedCommand(navStopDistance);
Behavior b = new StayInLaneBehavior(lane.LaneId, new StopAtDistSpeedCommand(navStopDistance), new List<int>(), lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false));
// stopping so not using speed param
usingSpeed = false;
IState nextState = CoreCommon.CorePlanningState;
m = new Maneuver(b, nextState, decorators, state.Timestamp);
}
#endregion
#region Outisde Distance Envelope
// not inside distance envalope
else
{
// get lane
ArbiterLane al = lane;
// default behavior
tp.SpeedCommand = new ScalarSpeedCommand(Math.Min(navStopSpeed, 2.24));
Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(Math.Min(navStopSpeed, 2.24)), new List<int>(), lp, al.Width, al.NumberOfLanesRight(state.Front, false), al.NumberOfLanesLeft(state.Front, false));
// standard behavior is fine for maneuver
m = new Maneuver(b, CoreCommon.CorePlanningState, decorators, state.Timestamp);
}
#endregion
#endregion
#region Parameterize
tp.Behavior = m.PrimaryBehavior;
tp.Decorators = m.PrimaryBehavior.Decorators;
tp.DistanceToGo = navStopDistance;
tp.NextState = m.PrimaryState;
tp.RecommendedSpeed = navStopSpeed;
tp.Type = TravellingType.Navigation;
tp.UsingSpeed = usingSpeed;
tp.VehiclesToIgnore = new List<int>();
// return navigation params
return tp;
#endregion
}
/// <summary>
/// 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>
/// Parameters to follow the forward vehicle
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
/// <returns></returns>
public TravelingParameters Follow(ArbiterLane lane, VehicleState state)
{
// travelling parameters
TravelingParameters tp = new TravelingParameters();
// ignorable obstacles
List<int> ignoreVehicle = new List<int>();
ignoreVehicle.Add(CurrentVehicle.VehicleId);
// get control parameters
ForwardVehicleTrackingControl fvtc = GetControl(lane, state);
// init params
tp.DistanceToGo = fvtc.xDistanceToGood;
tp.NextState = CoreCommon.CorePlanningState;
tp.RecommendedSpeed = fvtc.vFollowing;
tp.Type = TravellingType.Vehicle;
tp.Decorators = TurnDecorators.NoDecorators;
// flag to ignore forward vehicle
bool ignoreForward = false;
// reversed lane path
LinePath lp = lane.LanePath().Clone();
lp.Reverse();
#region Following Control
#region Immediate Stop
// need to stop immediately
if (fvtc.vFollowing == 0.0)
{
// don't ignore forward
ignoreForward = false;
// speed command
SpeedCommand sc = new ScalarSpeedCommand(0.0);
tp.UsingSpeed = true;
// standard path following behavior
Behavior final = new StayInLaneBehavior(lane.LaneId, sc, ignoreVehicle, lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false));
tp.Behavior = final;
tp.SpeedCommand = sc;
}
#endregion
#region Distance Stop
// stop at distance
else if (fvtc.vFollowing < 0.7 &&
CoreCommon.Communications.GetVehicleSpeed().Value <= 2.24 &&
fvtc.xSeparation > fvtc.xAbsMin)
{
// ignore forward vehicle
ignoreForward = true;
// speed command
SpeedCommand sc = new StopAtDistSpeedCommand(fvtc.xDistanceToGood);
tp.UsingSpeed = false;
// standard path following behavior
Behavior final = new StayInLaneBehavior(lane.LaneId, sc, ignoreVehicle, lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false));
tp.Behavior = final;
tp.SpeedCommand = sc;
}
#endregion
#region Normal Following
// else normal
else
{
// ignore the forward vehicle as we are tracking properly
ignoreForward = true;
// speed command
SpeedCommand sc = new ScalarSpeedCommand(fvtc.vFollowing);
tp.DistanceToGo = fvtc.xDistanceToGood;
tp.NextState = CoreCommon.CorePlanningState;
tp.RecommendedSpeed = fvtc.vFollowing;
tp.Type = TravellingType.Vehicle;
tp.UsingSpeed = true;
// standard path following behavior
Behavior final = new StayInLaneBehavior(lane.LaneId, sc, ignoreVehicle, lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false));
tp.Behavior = final;
tp.SpeedCommand = sc;
}
#endregion
#endregion
// check ignore
if (ignoreForward)
{
List<int> ignorable = new List<int>();
ignorable.Add(this.CurrentVehicle.VehicleId);
tp.VehiclesToIgnore = ignorable;
}
else
tp.VehiclesToIgnore = new List<int>();
// return parameterization
return tp;
}
/// <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;
}
public int NumberOfLanesLeft(Coordinates position, bool forwards)
{
if (forwards)
{
if (this.LaneOnLeft != null)
{
// our lane #
int n = this.LaneId.Number;
// left lane #
int l = this.LaneOnLeft.LaneId.Number;
// count
int count = 0;
// if left lane numbers going down
if (n > l)
{
// loop over left lanes going down
for (int i = n - 1; i > 0; i--)
{
// determine who is close to this position
ArbiterLaneId lane1 = new ArbiterLaneId(i, this.Way.Segment.Way1.WayId);
ArbiterLaneId lane2 = new ArbiterLaneId(i, this.Way.Segment.Way2.WayId);
// get lane
ArbiterLane lane = null;
if (this.Way.Segment.Lanes.ContainsKey(lane1))
{
lane = this.Way.Segment.Lanes[lane1];
}
else if (this.Way.Segment.Lanes.ContainsKey(lane2))
{
lane = this.Way.Segment.Lanes[lane2];
}
// check lane exists
if (lane != null)
{
// determine if close and inside
bool close = lane.LanePath().GetClosestPoint(position).Location.DistanceTo(position) < 15;
bool inside = lane.IsInside(position);
// add if both
if (close && inside)
{
count++;
}
}
}
}
else
{
// loop over left lanes
for (int i = n + 1; i <= this.Way.Segment.Lanes.Count; i++)
{
// determine who is close to this position
ArbiterLaneId lane1 = new ArbiterLaneId(i, this.Way.Segment.Way1.WayId);
ArbiterLaneId lane2 = new ArbiterLaneId(i, this.Way.Segment.Way2.WayId);
// get lane
ArbiterLane lane = null;
if (this.Way.Segment.Lanes.ContainsKey(lane1))
{
lane = this.Way.Segment.Lanes[lane1];
}
else if (this.Way.Segment.Lanes.ContainsKey(lane2))
{
lane = this.Way.Segment.Lanes[lane2];
}
// check lane exists
if (lane != null)
{
// determine if close and inside
bool close = lane.LanePath().GetClosestPoint(position).Location.DistanceTo(position) < 15;
bool inside = lane.IsInside(position);
// add if both
if (close && inside)
{
count++;
}
}
}
}
// return the count
return(count);
}
else
{
return(0);
}
}
else
{
return(this.NumberOfLanesRight(position, true));
}
}
/// <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;
}
}
