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>
/// 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>
/// 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>
/// 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);
}
}
/// <summary>
/// Reverse because of a blockage
/// </summary>
/// <param name="lane"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicleSpeed"></param>
/// <param name="defcon"></param>
/// <param name="saudi"></param>
/// <param name="laneOpposing"></param>
/// <param name="currentBlockage"></param>
/// <param name="ebs"></param>
/// <returns></returns>
private Maneuver LaneReverseManeuver(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed,
BlockageRecoveryDEFCON defcon, SAUDILevel saudi, bool laneOpposing, ITacticalBlockage currentBlockage, EncounteredBlockageState ebs)
{
// distance to reverse
double distToReverse = TahoeParams.VL * 2.0;
// just reverse and let brian catch it
StayInLaneBehavior reverseBehavior = new StayInLaneBehavior(
lane.LaneId, new StopAtDistSpeedCommand(distToReverse, true),
new List<int>(), lane.LanePath(), lane.Width,
lane.NumberOfLanesLeft(vehicleState.Front, true), lane.NumberOfLanesRight(vehicleState.Front, true));
// get the saudi level
List<BehaviorDecorator> decs = new List<BehaviorDecorator>(TurnDecorators.HazardDecorator.ToArray());
decs.Add(new ShutUpAndDoItDecorator(saudi));
reverseBehavior.Decorators = decs;
// state
IState laneState = new StayInLaneState(lane, CoreCommon.CorePlanningState);
BlockageRecoveryState brs = new BlockageRecoveryState(
reverseBehavior, laneState, laneState, defcon, ebs, BlockageRecoverySTATUS.EXECUTING);
// check enough room in lane to reverse
RearQuadrantMonitor rearMonitor = this.tacticalUmbrella.roadTactical.forwardReasoning.RearMonitor;
if (rearMonitor == null || !rearMonitor.lane.Equals(lane))
this.tacticalUmbrella.roadTactical.forwardReasoning.RearMonitor = new RearQuadrantMonitor(lane, SideObstacleSide.Driver);
#region Start too close to start of lane
// check distance to the start of the lane
double laneStartDistanceFromFront = lane.DistanceBetween(lane.WaypointList[0].Position, vehicleState.Front);
if (laneStartDistanceFromFront < TahoeParams.VL)
{
// make sure we're not at the very start of the lane
if (laneStartDistanceFromFront < 0.5)
{
// output
ArbiterOutput.Output("Too close to the start of the lane, raising defcon");
// go up chain
return new Maneuver(new NullBehavior(),
new EncounteredBlockageState(currentBlockage, laneState, BlockageRecoveryDEFCON.WIDENBOUNDS, saudi),
TurnDecorators.NoDecorators,
vehicleState.Timestamp);
}
// otherwise back up to the start of the lane
else
{
// output
ArbiterOutput.Output("Too close to the start of the lane, setting reverse distance to TP.VL");
// set proper distance
distToReverse = TahoeParams.VL;
// set behavior speed (no car behind us as too close to start of lane)
LinePath bp = vehicleState.VehicleLinePath;
reverseBehavior.SpeedCommand = new StopAtDistSpeedCommand(distToReverse, true);
StayInLaneBehavior silb = new StayInLaneBehavior(null, reverseBehavior.SpeedCommand, new List<int>(), bp, lane.Width * 1.5, 0, 0);
return new Maneuver(silb, brs, decs, vehicleState.Timestamp);
}
}
#endregion
#region Sparse
// check distance to the start of the lane
if (lane.GetClosestPartition(vehicleState.Front).Type == PartitionType.Sparse)
{
// set behavior speed (no car behind us as too close to start of lane)
LinePath bp = vehicleState.VehicleLinePath;
reverseBehavior.SpeedCommand = new StopAtDistSpeedCommand(distToReverse, true);
StayInLaneBehavior silb = new StayInLaneBehavior(null, reverseBehavior.SpeedCommand, new List<int>(), bp, lane.Width * 1.5, 0, 0);
return new Maneuver(silb, brs, decs, vehicleState.Timestamp);
}
#endregion
#region Vehicle Behind us
// update and check for vehicle
rearMonitor.Update(vehicleState);
if (rearMonitor.CurrentVehicle != null)
{
// check for not failed forward vehicle
if (rearMonitor.CurrentVehicle.QueuingState.Queuing != QueuingState.Failed)
{
// check if enough room to rear vehicle
double vehicleDistance = lane.DistanceBetween(rearMonitor.CurrentVehicle.ClosestPosition, vehicleState.Rear);
if (vehicleDistance < distToReverse - 2.0)
{
// revised distance given vehicle
double revisedDistance = vehicleDistance - 2.0;
// check enough room
if (revisedDistance > TahoeParams.VL)
{
// set the updated speed command
reverseBehavior.SpeedCommand = new StopAtDistSpeedCommand(revisedDistance, true);
}
// not enough room
else
{
// output not enough room because of vehicle
ArbiterOutput.Output("Blockage recovery, not enough room in rear because of rear vehicle, waiting for it to clear");
return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
}
// check failed rear vehicle, up defcon
else
{
// failed vehicle in rear, go up chain
return new Maneuver(new NullBehavior(),
new EncounteredBlockageState(currentBlockage, laneState, BlockageRecoveryDEFCON.WIDENBOUNDS, saudi),
TurnDecorators.NoDecorators,
vehicleState.Timestamp);
}
}
#endregion
// all clear, return reverse maneuver, set cooldown
BlockageHandler.SetDefaultBlockageCooldown();
return new Maneuver(reverseBehavior, brs, decs, vehicleState.Timestamp);
}
/// <summary>
/// Determine a change lanes recovery behavior
/// </summary>
/// <param name="current"></param>
/// <param name="vs"></param>
/// <param name="shouldWait"></param>
/// <returns></returns>
private ChangeLaneBehavior ChangeLanesRecoveryBehavior(ArbiterLane current, VehicleState vs, out bool shouldWait, out IState completionState)
{
// set defaults
shouldWait = false;
completionState = null;
// check partition type to make sure normal
if(current.GetClosestPartition(vs.Front).Type != PartitionType.Normal)
return null;
// make sure not in a safety zone
foreach(ArbiterSafetyZone asz in current.SafetyZones)
{
if(asz.IsInSafety(vs.Front))
return null;
}
// check not inside an intersection safety zone
foreach(ArbiterIntersection aInter in current.Way.Segment.RoadNetwork.ArbiterIntersections.Values)
{
if(aInter.IntersectionPolygon.IsInside(vs.Front) && (aInter.StoppedExits != null && aInter.StoppedExits.Count > 0))
return null;
}
// get the distance to the next lane major
ArbiterWaypoint nextWaypoint = current.GetClosestPartition(vs.Front).Final;
List<WaypointType> majorLaneTypes = new List<WaypointType>(new WaypointType[]{ WaypointType.End, WaypointType.Stop});
double distanceToNextMajor = current.DistanceBetween(vs.Front, current.GetNext(nextWaypoint, majorLaneTypes, new List<ArbiterWaypoint>()).Position);
// check distance > 3.0
if(distanceToNextMajor > 30.0)
{
// check clear on right, or if it does not exist here
ILateralReasoning rightLateral = this.tacticalUmbrella.roadTactical.rightLateralReasoning;
bool useRight = rightLateral.Exists && rightLateral.ExistsExactlyHere(vs) && rightLateral is LateralReasoning;
// check clear on left, or if it does not exist here
ILateralReasoning leftLateral = this.tacticalUmbrella.roadTactical.leftLateralReasoning;
bool useLeft = leftLateral.Exists && leftLateral.ExistsExactlyHere(vs) && leftLateral is LateralReasoning;
// notify
ArbiterOutput.Output("Blockage recovery: lane change left ok to use: " + useLeft.ToString() + ", use righrt: " + useRight.ToString());
#region Test Right
// determine if should use, should wait, or should not use
LaneChangeMonitorResult rightLCMR = LaneChangeMonitorResult.DontUse;
// check usability of the right lateral maneuver
if(useRight && rightLateral is LateralReasoning)
{
// check adj and rear clear
bool adjRearClear = rightLateral.AdjacentAndRearClear(vs);
// wait maybe if not clear
if(!adjRearClear && (rightLateral.AdjacentVehicle == null || rightLateral.AdjacentVehicle.QueuingState.Queuing != QueuingState.Failed))
rightLCMR = LaneChangeMonitorResult.Wait;
else if(adjRearClear)
rightLCMR = LaneChangeMonitorResult.Use;
else
rightLCMR = LaneChangeMonitorResult.DontUse;
// check down
if (rightLCMR == LaneChangeMonitorResult.Wait)
{
ArbiterOutput.Output("Blockage lane changes waiting for right lateral lane to clear");
shouldWait = true;
return null;
}
// check use
else if (rightLCMR == LaneChangeMonitorResult.Use)
{
// get the behavior
ChangeLaneBehavior rightClb = new ChangeLaneBehavior(current.LaneId, rightLateral.LateralLane.LaneId, false,
TahoeParams.VL * 1.5, new ScalarSpeedCommand(2.24), new List<int>(), current.LanePath(), rightLateral.LateralLane.LanePath(),
current.Width, rightLateral.LateralLane.Width, current.NumberOfLanesLeft(vs.Front, true), current.NumberOfLanesRight(vs.Front, true));
// test
CompletionReport rightClbCr;
bool successCL = CoreCommon.Communications.TestExecute(rightClb, out rightClbCr);
if (successCL && rightClbCr is SuccessCompletionReport)
{
ArbiterOutput.Output("Blockage lane changes found good behavior to right lateral lane");
shouldWait = false;
completionState = new StayInLaneState(rightLateral.LateralLane, CoreCommon.CorePlanningState);
return rightClb;
}
}
}
#endregion
#region Test Left Forwards
if(useLeft && leftLateral is LateralReasoning)
{
// determine if should use, should wait, or should not use
LaneChangeMonitorResult leftLCMR = LaneChangeMonitorResult.DontUse;
// check usability of the left lateral maneuver
if (useLeft && leftLateral is LateralReasoning)
{
// check adj and rear clear
bool adjRearClear = leftLateral.AdjacentAndRearClear(vs);
// wait maybe if not clear
if (!adjRearClear && (leftLateral.AdjacentVehicle == null || leftLateral.AdjacentVehicle.QueuingState.Queuing != QueuingState.Failed))
leftLCMR = LaneChangeMonitorResult.Wait;
else if (adjRearClear)
leftLCMR = LaneChangeMonitorResult.Use;
else
leftLCMR = LaneChangeMonitorResult.DontUse;
ArbiterOutput.Output("Blockage recovery, lane change left: " + leftLCMR.ToString());
// check down
if (leftLCMR == LaneChangeMonitorResult.Wait)
{
ArbiterOutput.Output("Blockage recovery, Blockage lane changes waiting for left lateral lane to clear");
shouldWait = true;
return null;
}
// check use
else if (leftLCMR == LaneChangeMonitorResult.Use)
{
// get the behavior
ChangeLaneBehavior leftClb = new ChangeLaneBehavior(current.LaneId, leftLateral.LateralLane.LaneId, false,
TahoeParams.VL * 1.5, new ScalarSpeedCommand(2.24), new List<int>(), current.LanePath(), leftLateral.LateralLane.LanePath(),
current.Width, leftLateral.LateralLane.Width, current.NumberOfLanesLeft(vs.Front, true), current.NumberOfLanesLeft(vs.Front, true));
// test
CompletionReport leftClbCr;
bool successCL = CoreCommon.Communications.TestExecute(leftClb, out leftClbCr);
if (successCL && leftClbCr is SuccessCompletionReport)
{
ArbiterOutput.Output("Blockage recovery, Blockage lane changes found good behavior to left lateral lane");
completionState = new StayInLaneState(leftLateral.LateralLane, CoreCommon.CorePlanningState);
shouldWait = false;
return leftClb;
}
}
}
}
#endregion
}
// fallout return null
return null;
}
/// <summary>
/// Gets the next navigational stop relavant to us (stop or end) in the closest good lane or our current opposing lane
/// </summary>
/// <param name="closestGood"></param>
/// <param name="coordinates"></param>
/// <param name="ignorable"></param>
/// <param name="navStopSpeed"></param>
/// <param name="navStopDistance"></param>
/// <param name="navStopType"></param>
/// <param name="navStop"></param>
private void NextOpposingNavigationalStop(ArbiterLane opposing, ArbiterLane closestGood, Coordinates coordinates, double extraDistance,
out double navStopSpeed, out double navStopDistance, out StopType navStopType, out ArbiterWaypoint navStop)
{
ArbiterWaypoint current = null;
double minDist = Double.MaxValue;
StopType st = StopType.EndOfLane;
#region Closest Good Parameterization
foreach (ArbiterWaypoint aw in closestGood.WaypointList)
{
if (aw.IsStop || aw.NextPartition == null)
{
double dist = closestGood.DistanceBetween(coordinates, aw.Position);
if (dist < minDist && dist >= 0)
{
current = aw;
minDist = dist;
st = aw.IsStop ? StopType.StopLine : StopType.EndOfLane;
}
}
}
#endregion
#region Opposing Parameterization
ArbiterWaypoint opStart = opposing.GetClosestPartition(coordinates).Initial;
int startIndex = opposing.WaypointList.IndexOf(opStart);
for (int i = startIndex; i >= 0; i--)
{
ArbiterWaypoint aw = opposing.WaypointList[i];
if (aw.IsStop || aw.PreviousPartition == null)
{
double dist = opposing.DistanceBetween(aw.Position, coordinates);
if (dist < minDist && dist >= 0)
{
current = aw;
minDist = dist;
st = aw.IsStop ? StopType.StopLine : StopType.EndOfLane;
}
}
}
#endregion
double tmpDistanceIgnore;
this.StoppingParams(current, closestGood, coordinates, extraDistance, out navStopSpeed, out tmpDistanceIgnore);
navStop = current;
navStopDistance = minDist;
navStopType = st;
}
public bool VehiclePassableInPolygon(ArbiterLane al, Polygon p, VehicleState ourState, Polygon circ)
{
List<Coordinates> vhcCoords = new List<Coordinates>();
for (int i = 0; i < this.trackedCluster.relativePoints.Length; i++)
vhcCoords.Add(this.TransformCoordAbs(this.trackedCluster.relativePoints[i], ourState));
Polygon vehiclePoly = Polygon.GrahamScan(vhcCoords);
vehiclePoly = Polygon.ConvexMinkowskiConvolution(circ, vehiclePoly);
ArbiterLanePartition alp = al.GetClosestPartition(this.trackedCluster.closestPoint);
List<Coordinates> pointsOutside = new List<Coordinates>();
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>
/// Secondary maneuver when current lane is the desired lane
/// </summary>
/// <param name="arbiterLane"></param>
/// <param name="vehicleState"></param>
/// <param name="roadPlan"></param>
/// <param name="blockages"></param>
/// <param name="ignorable"></param>
/// <returns></returns>
public Maneuver? AdvancedSecondaryNextCheck(ArbiterLane lane, ArbiterWaypoint laneGoal, VehicleState vehicleState, RoadPlan roadPlan,
List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, TypeOfTasks bestTask)
{
// check if the forward vehicle exists
if (this.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker && this.ForwardMonitor.ForwardVehicle.CurrentVehicle.PassedLongDelayedBirth)
{
// distance to forward vehicle
double distanceToForwardVehicle = lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);
#region Distance From Forward Not Enough
// check distance to forward vehicle
if (this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.Queuing == QueuingState.Failed &&
distanceToForwardVehicle < 8.0 && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle)
{
// set name
ArbiterLane al = lane;
// distance to forward vehicle too small
double distToForwards = al.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);
double distToReverse = Math.Max(1.0, 8.0 - distToForwards);
if (distToForwards < 8.0)
{
// notify
ArbiterOutput.Output("Secondary: NOT Properly Stopped Behind Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " distance: " + distToForwards.ToString("f2"));
this.RearMonitor = new RearQuadrantMonitor(al, SideObstacleSide.Driver);
this.RearMonitor.Update(vehicleState);
if (this.RearMonitor.CurrentVehicle != null)
{
double distToRearVehicle = al.DistanceBetween(this.RearMonitor.CurrentVehicle.ClosestPosition, vehicleState.Position) - TahoeParams.RL;
double distNeedClear = distToReverse + 2.0;
if (distToRearVehicle < distNeedClear)
{
// notify
ArbiterOutput.Output("Secondary: Rear: Not enough room to clear in rear: " + distToRearVehicle.ToString("f2") + " < " + distNeedClear.ToString("f2"));
return null;
}
}
double distToLaneStart = al.DistanceBetween(al.LanePath().StartPoint.Location, vehicleState.Position) - TahoeParams.RL;
if (distToReverse > distToLaneStart)
{
// notify
ArbiterOutput.Output("Secondary: Rear: Not enough room in lane to reverse in rear: " + distToLaneStart.ToString("f2") + " < " + distToReverse.ToString("f2"));
return null;
}
else
{
// notify
ArbiterOutput.Output("Secondary: Reversing to pass Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " reversing distance: " + distToReverse.ToString("f2"));
StopAtDistSpeedCommand sadsc = new StopAtDistSpeedCommand(distToReverse, true);
StayInLaneBehavior silb = new StayInLaneBehavior(al.LaneId, sadsc, new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(vehicleState.Front, true), al.NumberOfLanesRight(vehicleState.Front, true));
return new Maneuver(silb, CoreCommon.CorePlanningState, TurnDecorators.HazardDecorator, vehicleState.Timestamp);
}
}
}
#endregion
// get distance to next lane major
List<WaypointType> wts = new List<WaypointType>(new WaypointType[] { WaypointType.Stop, WaypointType.End });
ArbiterWaypoint nextWaypoint = lane.GetNext(lane.GetClosestPartition(vehicleState.Position).Final, wts, new List<ArbiterWaypoint>());
// check if the vehicle occurs before the next major thing
double distanceToNextMajor = lane.DistanceBetween(vehicleState.Front, nextWaypoint.Position);
// check if vehicle occurs before the next lane major
if (distanceToForwardVehicle < distanceToNextMajor)
{
// check if forward vehicle exists in this lane and is closer then the lane goal
if (TacticalDirector.VehicleAreas.ContainsKey(lane) &&
TacticalDirector.VehicleAreas[lane].Contains(this.ForwardMonitor.ForwardVehicle.CurrentVehicle) &&
lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) < lane.DistanceBetween(vehicleState.Front, laneGoal.Position))
{
// some constants
double desiredDistance = 50.0;
// get the lane goal distance
double distanceToLaneGoal = lane.DistanceBetween(vehicleState.Front, laneGoal.Position);
// check if necessary to be in current lane
if (distanceToLaneGoal <= desiredDistance)
{
// default secondary maneuver
ArbiterOutput.WriteToLog("AdvancedSecondaryNextCheck: DistToLaneGoal < 50: " + desiredDistance.ToString("f2"));
return this.SecondaryManeuver(lane, vehicleState, roadPlan, new List<ITacticalBlockage>(), new List<ArbiterWaypoint>(), bestTask);
}
// otherwise would be good but not necessary
else
{
// return the maneuver from the vehicle being outside the min cap
ArbiterOutput.WriteToLog("AdvancedSecondaryNextCheck: DistToLaneGoal > 50: " + desiredDistance.ToString("f2"));
return this.AdvancedSecondaryOutsideMinCap(lane, laneGoal, vehicleState, roadPlan, blockages, ignorable, bestTask);
}
}
}
}
// no secondary
return null;
}
/// <summary>
/// Distinctly want to make lane change, parameters for doing so
/// </summary>
/// <param name="arbiterLane"></param>
/// <param name="left"></param>
/// <param name="vehicleState"></param>
/// <param name="roadPlan"></param>
/// <param name="blockages"></param>
/// <param name="ignorable"></param>
/// <returns></returns>
public Maneuver? AdvancedDesiredLaneChangeManeuver(ArbiterLane lane, bool left, ArbiterWaypoint goal, RoadPlan rp, VehicleState vehicleState,
List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, LaneChangeInformation laneChangeInformation, Maneuver? secondary,
out LaneChangeParameters parameters)
{
// set aprams
parameters = new LaneChangeParameters();
// get final maneuver
Maneuver? final = null;
// check partition is not a startup chute
if (lane.GetClosestPartition(vehicleState.Front).Type != PartitionType.Startup)
{
// get the lane goal distance
double distanceToLaneGoal = lane.DistanceBetween(vehicleState.Front, goal.Position);
// check if our distance is less than 50m to the goal
if (distanceToLaneGoal < 50.0)
{
// use old
final = this.LaneChangeManeuver(lane, left, goal, vehicleState, blockages, ignorable, laneChangeInformation, secondary, out parameters);
try
{
// check final null
if (final == null)
{
// check for checkpoint within 4VL of front of failed vehicle
ArbiterCheckpoint acCurrecnt = CoreCommon.Mission.MissionCheckpoints.Peek();
if (acCurrecnt.WaypointId is ArbiterWaypointId)
{
// get waypoint
ArbiterWaypoint awCheckpoint = (ArbiterWaypoint)CoreCommon.RoadNetwork.ArbiterWaypoints[acCurrecnt.WaypointId];
// check way
if (awCheckpoint.Lane.Way.Equals(lane.Way))
{
// distance to wp
double distToWp = lane.DistanceBetween(vehicleState.Front, awCheckpoint.Position);
// check close to waypoint and stopped
if (CoreCommon.Communications.GetVehicleSpeed().Value < 0.1 && distToWp < TahoeParams.VL * 1.0)
{
ArbiterOutput.Output("Removing checkpoint: " + acCurrecnt.WaypointId.ToString() + " Stopped next to it");
CoreCommon.Mission.MissionCheckpoints.Dequeue();
return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
}
}
}
catch (Exception) { }
}
// no forward vehicle
else if (this.ForwardMonitor.ForwardVehicle.CurrentVehicle == null)
{
// adjacent monitor
LateralReasoning adjacent = null;
if (left && this.leftLateralReasoning is LateralReasoning && this.leftLateralReasoning.Exists)
{
// update
adjacent = (LateralReasoning)this.leftLateralReasoning;
}
else if (!left && this.rightLateralReasoning is LateralReasoning && this.rightLateralReasoning.Exists)
{
// update
adjacent = (LateralReasoning)this.rightLateralReasoning;
}
// check adj
if (adjacent != null)
{
// update
adjacent.ForwardMonitor.Primary(adjacent.LateralLane, vehicleState, rp, new List<ITacticalBlockage>(), new List<ArbiterWaypoint>(), false);
if (adjacent.ForwardMonitor.ForwardVehicle.CurrentVehicle == null && adjacent.AdjacentAndRearClear(vehicleState))
{
// use old
final = this.LaneChangeManeuver(lane, left, goal, vehicleState, blockages, ignorable, laneChangeInformation, secondary, out parameters);
}
}
}
}
if (!final.HasValue)
{
if (!secondary.HasValue)
{
List<TravelingParameters> falloutParams = new List<TravelingParameters>();
TravelingParameters t1 = this.ForwardMonitor.ParameterizationHelper(lane, lane, goal.Position, vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, null);
falloutParams.Add(t1);
falloutParams.Add(this.ForwardMonitor.LaneParameters);
if (this.ForwardMonitor.FollowingParameters.HasValue)
falloutParams.Add(this.ForwardMonitor.FollowingParameters.Value);
falloutParams.Sort();
TravelingParameters tpCatch = falloutParams[0];
return new Maneuver(tpCatch.Behavior, tpCatch.NextState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
{
return secondary;
}
}
else
{
return final;
}
}
