/// <summary>
/// Constructor
/// </summary>
/// <param name="lane"></param>
public StayInLaneState(ArbiterLane lane, Probability confidence, IState previous)
{
this.Lane = lane;
this.internalLaneState = new InternalState(lane.LaneId, lane.LaneId, confidence);
this.IgnorableWaypoints = new List<IgnorableWaypoint>();
this.CheckPreviousState(previous);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="safetyZoneEnd"></param>
/// <param name="safetyZoneBegin"></param>
public ArbiterSafetyZone(ArbiterLane lane, LinePath.PointOnPath safetyZoneEnd, LinePath.PointOnPath safetyZoneBegin)
{
this.safetyZoneBegin = safetyZoneBegin;
this.safetyZoneEnd = safetyZoneEnd;
this.lane = lane;
this.GenerateSafetyZone();
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="lane"></param>
public OpposingLateralReasoning(ArbiterLane lane, SideObstacleSide sos)
{
this.lane = lane;
this.ForwardMonitor = new OpposingForwardQuadrantMonitor();
this.LateralMonitor = new OpposingLateralQuadrantMonitor(sos);
this.RearMonitor = new OpposingRearQuadrantMonitor(lane, sos);
}
/// <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>
/// Constructor
/// </summary>
/// <param name="safetyZoneEnd"></param>
/// <param name="safetyZoneBegin"></param>
public ArbiterSafetyZone(ArbiterLane lane, LinePath.PointOnPath safetyZoneEnd, LinePath.PointOnPath safetyZoneBegin)
{
this.safetyZoneBegin = safetyZoneBegin;
this.safetyZoneEnd = safetyZoneEnd;
this.lane = lane;
this.GenerateSafetyZone();
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="leftLateral"></param>
/// <param name="rightLateral"></param>
public OpposingReasoning(ILateralReasoning leftLateral, ILateralReasoning rightLateral, ArbiterLane lane)
{
this.Lane = lane;
this.leftLateralReasoning = leftLateral;
this.rightLateralReasoning = rightLateral;
this.OpposingForwardMonitor = new OpposingForwardQuadrantMonitor();
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="lane"></param>
/// <param name="stopPoint"></param>
/// <param name="currentPosition"></param>
/// <param name="timeStamp"></param>
public StoppingState(ArbiterLane lane, Coordinates stopPoint, Coordinates currentPosition, double timeStamp)
{
this.lane = lane;
this.stopPoint = stopPoint;
this.currentPosition = currentPosition;
this.timeStamp = timeStamp;
this.internalState = new InternalState(lane.LaneId, lane.LaneId);
}
/// <summary>
/// Plans the forward maneuver and secondary maneuver
/// </summary>
/// <param name="arbiterLane"></param>
/// <param name="vehicleState"></param>
/// <param name="p"></param>
/// <param name="blockage"></param>
/// <returns></returns>
public Maneuver ForwardManeuver(ArbiterLane arbiterLane, ArbiterLane closestGood, VehicleState vehicleState, RoadPlan roadPlan,
List<ITacticalBlockage> blockages)
{
// get primary maneuver
Maneuver primary = this.OpposingForwardMonitor.PrimaryManeuver(arbiterLane, closestGood, vehicleState, blockages);
// return primary for now
return primary;
}
/// <summary>
/// Generates adjacency of a partition to another lane
/// </summary>
/// <param name="alp"></param>
/// <param name="al"></param>
private void GenerateSinglePartitionAdjacency(ArbiterLanePartition alp, ArbiterLane al)
{
// fake path
List <IPathSegment> fakePathSegments = new List <IPathSegment>();
fakePathSegments.Add(new LinePathSegment(alp.Initial.Position, alp.Final.Position));
Path fakePath = new Path(fakePathSegments);
// partitions adjacent
List <ArbiterLanePartition> adjacent = new List <ArbiterLanePartition>();
// tracks
PointOnPath current = fakePath.StartPoint;
double increment = 0.5;
double tmpInc = 0;
// increment along
while (tmpInc == 0)
{
// loop over partitions
foreach (ArbiterLanePartition alpar in al.Partitions)
{
// get fake path for partition
List <IPathSegment> ips = new List <IPathSegment>();
ips.Add(new LinePathSegment(alpar.Initial.Position, alpar.Final.Position));
Path alpPath = new Path(ips);
// get closest point on tmp partition to current
PointOnPath closest = alpPath.GetClosest(current.pt);
// check general distance
if (closest.pt.DistanceTo(current.pt) < 20)
{
// check not start or end
if (!closest.Equals(alpPath.StartPoint) && !closest.Equals(alpPath.EndPoint))
{
// check not already added
if (!adjacent.Contains(alpar))
{
// add
adjacent.Add(alpar);
}
}
}
}
// set inc
tmpInc = increment;
// increment point
current = fakePath.AdvancePoint(current, ref tmpInc);
}
// add adjacent
alp.NonLaneAdjacentPartitions.AddRange(adjacent);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="lane"></param>
/// <param name="waypoint"></param>
/// <param name="turnDirection"></param>
/// <param name="isNavigationExit"></param>
/// <param name="desiredExit"></param>
public StoppingAtStopState(ArbiterLane lane, ArbiterWaypoint waypoint, ArbiterTurnDirection turnDirection,
bool isNavigationExit, ArbiterInterconnect desiredExit)
{
this.lane = lane;
this.waypoint = waypoint;
this.turnDirection = turnDirection;
this.isNavigationExit = isNavigationExit;
this.desiredExit = desiredExit;
this.internalLaneState = new InternalState(lane.LaneId, lane.LaneId);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="interconnect"></param>
/// <param name="turn"></param>
public TurnState(ArbiterInterconnect interconnect, ArbiterTurnDirection turn, ArbiterLane target, LinePath endingPath, LineList left,
LineList right, SpeedCommand speed, SAUDILevel saudi, bool useTurnBounds)
{
this.Interconnect = interconnect;
this.turnDirection = turn;
this.TargetLane = target;
this.EndingPath = endingPath;
this.LeftBound = left;
this.RightBound = right;
this.SpeedCommand = speed;
this.Saudi = saudi;
this.UseTurnBounds = useTurnBounds;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="lane"></param>
/// <param name="waypoint"></param>
/// <param name="turnDirection"></param>
/// <param name="isNavigationExit"></param>
/// <param name="desiredExit"></param>
public StoppingAtExitState(ArbiterLane lane, ArbiterWaypoint waypoint, ArbiterTurnDirection turnDirection,
bool isNavigationExit, ArbiterInterconnect desiredExit, double timeStamp, Coordinates currentPosition)
{
this.lane = lane;
this.waypoint = waypoint;
this.turnDirection = turnDirection;
this.isNavigationExit = isNavigationExit;
this.desiredExit = desiredExit;
this.internalLaneState = new InternalState(lane.LaneId, lane.LaneId);
this.timeStamp = timeStamp;
this.currentPosition = currentPosition;
this.internalLaneState = new InternalState(this.lane.LaneId, this.lane.LaneId);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="globalMonitor"></param>
/// <param name="involved"></param>
public DominantLaneEntryMonitor(IntersectionMonitor globalMonitor, IntersectionInvolved involved)
{
this.waitingTimer = new Stopwatch();
this.globalMonitor = globalMonitor;
this.lane = (ArbiterLane)involved.Area;
this.involved = involved;
if (involved.Exit != null)
this.exit = (ArbiterWaypoint)involved.Exit;
else
this.exit = null;
if (this.exit != null)
{
// create the polygon
this.exitPolygon = this.ExitPolygon();
}
}
/// <summary>
/// Gets closest lane to a point
/// </summary>
/// <param name="point"></param>
/// <returns></returns>
public ArbiterLane ClosestLane(Coordinates point)
{
ArbiterLane closest = null;
double best = Double.MaxValue;
foreach (ArbiterSegment asg in ArbiterSegments.Values)
{
foreach (ArbiterLane al in asg.Lanes.Values)
{
PointOnPath closestPoint = al.PartitionPath.GetClosest(point);
double tmp = closestPoint.pt.DistanceTo(point);
if (tmp < best)
{
closest = al;
best = tmp;
}
}
}
return(closest);
}
public OpposingLanesState(ArbiterLane opposingLane, bool resetLaneAgent, IState previous, VehicleState vs)
{
this.resetLaneAgent = resetLaneAgent;
this.OpposingLane = opposingLane;
this.OpposingWay = opposingLane.Way.WayId;
this.ClosestGoodLane = null;
this.SetClosestGood(vs);
if (previous is ChangeLanesState)
{
EntryParameters = ((ChangeLanesState)previous).Parameters;
}
else if (previous is OpposingLanesState)
{
EntryParameters = ((OpposingLanesState)previous).EntryParameters;
}
else
{
EntryParameters = null;
}
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initial"></param>
/// <param name="next"></param>
/// <param name="final"></param>
public SupraLane(ArbiterLane initial, ArbiterInterconnect next, ArbiterLane final)
{
Components = new SupraLaneComponentList();
Components.Add(initial);
Components.Add(next);
Components.Add(final);
this.Initial = initial;
this.Final = final;
this.Interconnect = next;
if (!this.Initial.Equals(this.Final))
{
this.supraWaypoints = new List <ArbiterWaypoint>();
this.supraWaypoints.AddRange(this.Initial.WaypointsInclusive(this.Initial.WaypointList[0], (ArbiterWaypoint)this.Interconnect.InitialGeneric));
this.supraWaypoints.AddRange(this.Final.WaypointsInclusive((ArbiterWaypoint)this.Interconnect.FinalGeneric, this.Final.WaypointList[this.Final.WaypointList.Count - 1]));
this.SetDefaultLanePath();
}
else
{
this.supraWaypoints = this.Initial.WaypointList;
this.supraPath = this.Initial.LanePath();
this.supraPath.Add(this.Interconnect.FinalGeneric.Position);
}
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initial"></param>
/// <param name="next"></param>
/// <param name="final"></param>
public SupraLane(ArbiterLane initial, ArbiterInterconnect next, ArbiterLane final)
{
Components = new SupraLaneComponentList();
Components.Add(initial);
Components.Add(next);
Components.Add(final);
this.Initial = initial;
this.Final = final;
this.Interconnect = next;
if (!this.Initial.Equals(this.Final))
{
this.supraWaypoints = new List<ArbiterWaypoint>();
this.supraWaypoints.AddRange(this.Initial.WaypointsInclusive(this.Initial.WaypointList[0], (ArbiterWaypoint)this.Interconnect.InitialGeneric));
this.supraWaypoints.AddRange(this.Final.WaypointsInclusive((ArbiterWaypoint)this.Interconnect.FinalGeneric, this.Final.WaypointList[this.Final.WaypointList.Count - 1]));
this.SetDefaultLanePath();
}
else
{
this.supraWaypoints = this.Initial.WaypointList;
this.supraPath = this.Initial.LanePath();
this.supraPath.Add(this.Interconnect.FinalGeneric.Position);
}
}
/// <summary>
/// Check if a side sick blocking obstacle is detected
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
/// <returns></returns>
private bool SideSickObstacleDetected(ArbiterLane lane, VehicleState state)
{
try
{
// get distance from vehicle to lane opp side
Coordinates vec = state.Front - state.Position;
vec = this.VehicleSide == SideObstacleSide.Driver ? vec.Rotate90() : vec.RotateM90();
SideObstacles sobs = CoreCommon.Communications.GetSideObstacles(this.VehicleSide);
if (sobs != null && sobs.obstacles != null)
{
foreach (SideObstacle so in sobs.obstacles)
{
Coordinates cVec = state.Position + vec.Normalize(so.distance);
if (so.height > 0.7 && lane.LanePolygon.IsInside(cVec))
return true;
}
}
}
catch (Exception ex)
{
ArbiterOutput.Output("side sick obstacle exception: " + ex.ToString());
}
return false;
}
/// <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>
/// Check if found a stop in a lane as part of exits or a test waypoint
/// </summary>
/// <param name="initialTest"></param>
/// <param name="exits"></param>
/// <param name="lane"></param>
/// <returns></returns>
private bool FoundStop(ArbiterWaypoint initialTest, IEnumerable<ITraversableWaypoint> exits, ArbiterLane lane)
{
if (initialTest != null && initialTest.IsStop)
return true;
foreach (ITraversableWaypoint exit in exits)
{
if (exit is ArbiterWaypoint)
{
ArbiterWaypoint aw = (ArbiterWaypoint)exit;
if (aw.IsStop && aw.Lane.Equals(lane))
{
return true;
}
}
}
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
}
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>
/// 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>
/// Constructor
/// </summary>
/// <param name="lane"></param>
public RearQuadrantMonitor(ArbiterLane lane, SideObstacleSide sos)
{
this.lane = lane;
this.RearClearStopwatch = new Stopwatch();
this.VehicleSide = sos;
}
// Is the posterior evidence consistent over time?
private bool Ct(List<PosteriorEvidence> e1t)
{
ArbiterLane lane = null;
if (e1t.Count == 50)
{
// check if previous are consistent
for (int i = 0; i < e1t.Count; i++)
{
if (e1t[i].LaneProbabilities.Count > 0)
{
double max = Double.MinValue;
ArbiterLane curLane = null;
foreach (KeyValuePair<ArbiterLane, double> est in e1t[i].LaneProbabilities)
{
if (est.Value > max || curLane == null)
{
max = est.Value;
curLane = est.Key;
}
}
if (lane == null)
lane = curLane;
else
{
if (!lane.Equals(curLane))
{
return false;
}
}
}
}
if (lane == null)
return false;
else
{
this.sceneLikelyLane = lane;
CoreCommon.CurrentInformation.LASceneLikelyLane = this.sceneLikelyLane.ToString();
return true;
}
}
else
{
this.sceneLikelyLane = null;
return false;
}
}
public void SetClosestGood(VehicleState vs)
{
ArbiterLane current = this.OpposingLane.LaneOnLeft;
while (current != null)
{
if (!current.Way.WayId.Equals(OpposingWay) && current.RelativelyInside(vs.Front))
{
this.ClosestGoodLane = current;
break;
}
if (!current.Way.WayId.Equals(this.OpposingLane.Way.WayId))
current = current.LaneOnRight;
else
current = current.LaneOnLeft;
}
}
/// <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 if the reverse maneuver is blocked
/// </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 LaneReverseBlockedManeuver(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed,
BlockageRecoveryState brs, EncounteredBlockageState ebs, INavigationalPlan plan)
{
// get closest partition
ArbiterLanePartition alp = lane.GetClosestPartition(vehicleState.Front);
// check type
if (alp.Type == PartitionType.Sparse)
{
#region Get Recovery Behavior
// get the recovery behavior
StayInLaneBehavior testForwards = this.LaneRecoveryBehavior(lane, vehicleState, vehicleSpeed, plan, brs, ebs);
// up the saudi level
SAUDILevel sl = brs.EncounteredState.Saudi < SAUDILevel.L2 ? brs.EncounteredState.Saudi++ : SAUDILevel.L2;
testForwards.Decorators = new List<BehaviorDecorator>(new BehaviorDecorator[] { new ShutUpAndDoItDecorator(sl) });
// return the behavior
brs.EncounteredState.Saudi = sl;
brs.RecoveryStatus = BlockageRecoverySTATUS.EXECUTING;
return new Maneuver(testForwards, brs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
#endregion
}
else
{
return new Maneuver(new NullBehavior(), brs.AbortState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
public bool VehiclePassableInPolygon(ArbiterLane al, VehicleAgent va, Polygon p, VehicleState ourState, Polygon circ)
{
Polygon vehiclePoly = va.GetAbsolutePolygon(ourState);
vehiclePoly = Polygon.ConvexMinkowskiConvolution(circ, vehiclePoly);
List<Coordinates> pointsOutside = new List<Coordinates>();
ArbiterLanePartition alp = al.GetClosestPartition(va.ClosestPosition);
foreach(Coordinates c in vehiclePoly)
{
if (!p.IsInside(c))
pointsOutside.Add(c);
}
foreach (Coordinates m in pointsOutside)
{
foreach (Coordinates n in pointsOutside)
{
if(!m.Equals(n))
{
if (GeneralToolkit.TriangleArea(alp.Initial.Position, m, alp.Final.Position) *
GeneralToolkit.TriangleArea(alp.Initial.Position, n, alp.Final.Position) < 0)
{
return false;
}
}
}
}
return true;
}
/// <summary>
/// What to do when we complete a reverse maneuver
/// </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 LaneReverseCompleteManeuver(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed,
BlockageRecoveryState brs, EncounteredBlockageState ebs, INavigationalPlan plan)
{
// get the lane recovery behavior
StayInLaneBehavior testForwards = this.LaneRecoveryBehavior(lane, vehicleState, vehicleSpeed, plan, brs, ebs);
#region Test and Return
// notify
ArbiterOutput.Output("Attempting to test execute recovery complete behavior: " + testForwards.ToString());
// test the test behavior
CompletionReport testForwardsReport;
bool canCompleteTestForwards = CoreCommon.Communications.TestExecute(testForwards, out testForwardsReport);
// notify
ArbiterOutput.Output("Received completion result ok: " + canCompleteTestForwards.ToString() + " with completion result: " + testForwardsReport.Result.ToString());
// check completion ok of stop at distance
if (canCompleteTestForwards)
{
// switch to the completion state
brs.RecoveryStatus = BlockageRecoverySTATUS.EXECUTING;
return new Maneuver(testForwards, brs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// not ok, report this blockage
else
{
// return the blocked reversal maneuver
return this.LaneReverseBlockedManeuver(lane, vehicleState, vehicleSpeed, brs, ebs, plan);
}
#endregion
}
/// <summary>
/// Generates lane adjacency in the network
/// </summary>
/// <param name="arn"></param>
private void GenerateLaneAdjacency(ArbiterRoadNetwork arn)
{
// loop over segments
foreach (ArbiterSegment asg in arn.ArbiterSegments.Values)
{
// check if both ways exist for first algorithm
if (asg.Way1.IsValid && asg.Way2.IsValid)
{
// lanes of the segment
Dictionary <ArbiterLaneId, ArbiterLane> segLanes = asg.Lanes;
// get a sample lane from way 1
Dictionary <ArbiterLaneId, ArbiterLane> .Enumerator way1Enum = asg.Way1.Lanes.GetEnumerator();
way1Enum.MoveNext();
ArbiterLane way1Sample = way1Enum.Current.Value;
// get a sample lane from way 2
Dictionary <ArbiterLaneId, ArbiterLane> .Enumerator way2Enum = asg.Way2.Lanes.GetEnumerator();
way2Enum.MoveNext();
ArbiterLane way2Sample = way2Enum.Current.Value;
// direction, 1 means way1 has lower # lanes
int modifier = 1;
// check if way 2 has lower lane numbers
if (way1Sample.LaneId.Number > way2Sample.LaneId.Number)
{
// set modifier to -1 so count other way
modifier = -1;
}
// loop over lanes
foreach (ArbiterLane al in segLanes.Values)
{
// if not lane 1
if (al.LaneId.Number != 1)
{
// get lower # lane in way 1
ArbiterLaneId lowerNumWay1Id = new ArbiterLaneId(al.LaneId.Number - 1, asg.Way1.WayId);
// check if the segment contains this lane
if (segLanes.ContainsKey(lowerNumWay1Id))
{
// get lane
ArbiterLane lowerNumWay1 = segLanes[lowerNumWay1Id];
// check if current lane is also in way 1
if (lowerNumWay1.Way.WayId.Equals(al.Way.WayId))
{
// check modifier for 1 => lower is to right
if (modifier == 1)
{
al.LaneOnRight = lowerNumWay1;
lowerNumWay1.LaneOnLeft = al;
}
// otherwise -1 => lane is to left
else
{
al.LaneOnLeft = lowerNumWay1;
lowerNumWay1.LaneOnRight = al;
}
}
// otherwise the current lane is in a different way
else
{
// the lane is to the left by default
al.LaneOnLeft = lowerNumWay1;
lowerNumWay1.LaneOnLeft = al;
}
}
// otherwise the lowe lane is in way 2
else
{
// set lane
ArbiterLane lowerNumWay2 = segLanes[new ArbiterLaneId(al.LaneId.Number - 1, asg.Way2.WayId)];
// check if current lane is also in way 2
if (lowerNumWay2.Way.WayId.Equals(al.Way.WayId))
{
// check modifier for 1 => lower is to left
if (modifier == 1)
{
al.LaneOnLeft = lowerNumWay2;
lowerNumWay2.LaneOnRight = al;
}
// otherwise -1 => lane is to right
else
{
al.LaneOnRight = lowerNumWay2;
lowerNumWay2.LaneOnLeft = al;
}
}
// otherwise the current lane is in a different way
else
{
// the lane is to the left by default
al.LaneOnLeft = lowerNumWay2;
lowerNumWay2.LaneOnLeft = al;
}
}
}
} // loop over lanes
} // both ways valid
// otherwise only a single way is valid
else
{
// lanes of the segment
Dictionary <ArbiterLaneId, ArbiterLane> segLanes = asg.Lanes;
// make sure more than one lane exists
if (segLanes.Count > 1)
{
// loop over lanes
foreach (ArbiterLane al in segLanes.Values)
{
// get theoretical id of lane one number up
ArbiterLaneId ali = new ArbiterLaneId(al.LaneId.Number + 1, al.LaneId.WayId);
// check if lane one number up exists
if (segLanes.ContainsKey(ali))
{
// get lane one number up
ArbiterLane alu = segLanes[ali];
// check # waypoints
if (al.Waypoints.Count > 1 && alu.Waypoints.Count > 1)
{
// get closest points on this lane and other lane
PointOnPath p1;
PointOnPath p2;
double distance;
CreationTools.GetClosestPoints(al.PartitionPath, alu.PartitionPath, out p1, out p2, out distance);
// get partition points of closest point on this lane
Coordinates partitionStart = p1.segment.Start;
Coordinates partitionEnd = p1.segment.End;
// get area of partition triangle
double triangeArea = CreationTools.TriangleArea(partitionStart, p2.pt, partitionEnd);
// determine if closest point on other lane is to the left or right of partition
bool onLeft = true;
if (triangeArea >= 0)
{
onLeft = false;
}
// set adjacency accordingly for both lanes
if (onLeft)
{
al.LaneOnLeft = alu;
alu.LaneOnRight = al;
}
// otherwise on right
else
{
al.LaneOnRight = alu;
alu.LaneOnLeft = al;
}
}
}
}
}
} // end single way only valid
// loop over lanes to print info on adjacency
/*foreach (ArbiterLane al in asg.Lanes.Values)
* {
* Console.Write(al.LaneId.ToString() + ": ");
*
* if (al.LaneOnLeft != null)
* {
* Console.Write("Left-" + al.LaneOnLeft.LaneId.ToString());
* }
*
* if (al.LaneOnRight != null)
* {
* Console.Write("Right-" + al.LaneOnRight.LaneId.ToString());
* }
*
* Console.Write("\n");
* }
*/
} // segment loop
}
/// <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>
/// 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>
/// Generates the xySegments into segments and inputs them into the input road network
/// </summary>
/// <param name="arn"></param>
/// <returns></returns>
public ArbiterRoadNetwork GenerateSegments(ArbiterRoadNetwork arn)
{
foreach (SimpleSegment ss in segments)
{
// seg
ArbiterSegmentId asi = new ArbiterSegmentId(int.Parse(ss.Id));
ArbiterSegment asg = new ArbiterSegment(asi);
arn.ArbiterSegments.Add(asi, asg);
asg.RoadNetwork = arn;
asg.SpeedLimits = new ArbiterSpeedLimit();
asg.SpeedLimits.MaximumSpeed = 13.4112; // 30mph max speed
// way1
ArbiterWayId awi1 = new ArbiterWayId(1, asi);
ArbiterWay aw1 = new ArbiterWay(awi1);
aw1.Segment = asg;
asg.Ways.Add(awi1, aw1);
asg.Way1 = aw1;
// way2
ArbiterWayId awi2 = new ArbiterWayId(2, asi);
ArbiterWay aw2 = new ArbiterWay(awi2);
aw2.Segment = asg;
asg.Ways.Add(awi2, aw2);
asg.Way2 = aw2;
// make lanes
foreach (SimpleLane sl in ss.Lanes)
{
// lane
ArbiterLaneId ali;
ArbiterLane al;
// get way of lane id
if (ss.Way1Lanes.Contains(sl))
{
ali = new ArbiterLaneId(GenerationTools.GetId(sl.Id)[1], awi1);
al = new ArbiterLane(ali);
aw1.Lanes.Add(ali, al);
al.Way = aw1;
}
else
{
ali = new ArbiterLaneId(GenerationTools.GetId(sl.Id)[1], awi2);
al = new ArbiterLane(ali);
aw2.Lanes.Add(ali, al);
al.Way = aw2;
}
// add to display
arn.DisplayObjects.Add(al);
// width
al.Width = sl.LaneWidth == 0 ? TahoeParams.T * 2.0 : sl.LaneWidth * 0.3048;
if (sl.LaneWidth == 0)
{
Console.WriteLine("lane: " + ali.ToString() + " contains no lane width, setting to 4m");
}
// lane boundaries
al.BoundaryLeft = this.GenerateLaneBoundary(sl.LeftBound);
al.BoundaryRight = this.GenerateLaneBoundary(sl.RightBound);
// add lane to seg
asg.Lanes.Add(ali, al);
// waypoints
List <ArbiterWaypoint> waypointList = new List <ArbiterWaypoint>();
// generate waypoints
foreach (SimpleWaypoint sw in sl.Waypoints)
{
// waypoint
ArbiterWaypointId awi = new ArbiterWaypointId(GenerationTools.GetId(sw.ID)[2], ali);
ArbiterWaypoint aw = new ArbiterWaypoint(sw.Position, awi);
aw.Lane = al;
// stop
if (sl.Stops.Contains(sw.ID))
{
aw.IsStop = true;
}
// checkpoint
foreach (SimpleCheckpoint sc in sl.Checkpoints)
{
if (sw.ID == sc.WaypointId)
{
aw.IsCheckpoint = true;
aw.CheckpointId = int.Parse(sc.CheckpointId);
arn.Checkpoints.Add(aw.CheckpointId, aw);
}
}
// add
asg.Waypoints.Add(awi, aw);
arn.ArbiterWaypoints.Add(awi, aw);
al.Waypoints.Add(awi, aw);
waypointList.Add(aw);
arn.DisplayObjects.Add(aw);
arn.LegacyWaypointLookup.Add(sw.ID, aw);
}
al.WaypointList = waypointList;
// lane partitions
List <ArbiterLanePartition> alps = new List <ArbiterLanePartition>();
al.Partitions = alps;
// generate lane partitions
for (int i = 0; i < waypointList.Count - 1; i++)
{
// create lane partition
ArbiterLanePartitionId alpi = new ArbiterLanePartitionId(waypointList[i].WaypointId, waypointList[i + 1].WaypointId, ali);
ArbiterLanePartition alp = new ArbiterLanePartition(alpi, waypointList[i], waypointList[i + 1], asg);
alp.Lane = al;
waypointList[i].NextPartition = alp;
waypointList[i + 1].PreviousPartition = alp;
alps.Add(alp);
arn.DisplayObjects.Add(alp);
// crete initial user partition
ArbiterUserPartitionId aupi = new ArbiterUserPartitionId(alp.PartitionId, waypointList[i].WaypointId, waypointList[i + 1].WaypointId);
ArbiterUserPartition aup = new ArbiterUserPartition(aupi, alp, waypointList[i], waypointList[i + 1]);
List <ArbiterUserPartition> aups = new List <ArbiterUserPartition>();
aups.Add(aup);
alp.UserPartitions = aups;
alp.SetDefaultSparsePolygon();
arn.DisplayObjects.Add(aup);
}
// path segments of lane
List <IPathSegment> ips = new List <IPathSegment>();
List <Coordinates> pathSegments = new List <Coordinates>();
pathSegments.Add(alps[0].Initial.Position);
// loop
foreach (ArbiterLanePartition alPar in alps)
{
ips.Add(new LinePathSegment(alPar.Initial.Position, alPar.Final.Position));
// make new segment
pathSegments.Add(alPar.Final.Position);
}
// generate lane partition path
LinePath partitionPath = new LinePath(pathSegments);
al.SetLanePath(partitionPath);
al.PartitionPath = new Path(ips, CoordinateMode.AbsoluteProjected);
// safeto zones
foreach (ArbiterWaypoint aw in al.Waypoints.Values)
{
if (aw.IsStop)
{
LinePath.PointOnPath end = al.GetClosestPoint(aw.Position);
double dist = -30;
LinePath.PointOnPath begin = al.LanePath().AdvancePoint(end, ref dist);
if (dist != 0)
{
begin = al.LanePath().StartPoint;
}
ArbiterSafetyZone asz = new ArbiterSafetyZone(al, end, begin);
asz.isExit = true;
asz.Exit = aw;
al.SafetyZones.Add(asz);
arn.DisplayObjects.Add(asz);
arn.ArbiterSafetyZones.Add(asz);
}
}
}
}
return(arn);
}
