/// <summary>
/// Add blockage to partition
/// </summary>
/// <param name="partition"></param>
/// <param name="c"></param>
public void AddBlockage(IConnectAreaWaypoints partition, Coordinates c, bool acrossSegment)
{
partition.Blockage.BlockageExists = true;
partition.Blockage.BlockageCoordinates = c;
partition.Blockage.SecondsSinceObserved = 0.0;
partition.Blockage.BlockageTimeCost = 1800;
if (partition.Blockage.BlockageHasExisted)
{
partition.Blockage.BlockageLifetime = partition.Blockage.BlockageLifetime * 4.0;
}
if (acrossSegment && partition is ArbiterLanePartition)
{
foreach (ArbiterLanePartition alp in ((ArbiterLanePartition)partition).NonLaneAdjacentPartitions)
{
if (alp.IsInside(c))
{
alp.Blockage.BlockageExists = true;
alp.Blockage.BlockageCoordinates = c;
alp.Blockage.SecondsSinceObserved = 0.0;
alp.Blockage.BlockageTimeCost = 1800;
if (alp.Blockage.BlockageHasExisted)
{
alp.Blockage.BlockageLifetime = partition.Blockage.BlockageLifetime * 4.0;
}
}
}
}
// reset navigation costs
this.currentTimes = new KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> >();
}
/// <summary>
/// Add blockage to partition
/// </summary>
/// <param name="partition"></param>
/// <param name="c"></param>
public void AddBlockage(IConnectAreaWaypoints partition, Coordinates c, bool acrossSegment)
{
partition.Blockage.BlockageExists = true;
partition.Blockage.BlockageCoordinates = c;
partition.Blockage.SecondsSinceObserved = 0.0;
partition.Blockage.BlockageTimeCost = 1800;
if (partition.Blockage.BlockageHasExisted)
partition.Blockage.BlockageLifetime = partition.Blockage.BlockageLifetime * 4.0;
if (acrossSegment && partition is ArbiterLanePartition)
{
foreach (ArbiterLanePartition alp in ((ArbiterLanePartition)partition).NonLaneAdjacentPartitions)
{
if (alp.IsInside(c))
{
alp.Blockage.BlockageExists = true;
alp.Blockage.BlockageCoordinates = c;
alp.Blockage.SecondsSinceObserved = 0.0;
alp.Blockage.BlockageTimeCost = 1800;
if (alp.Blockage.BlockageHasExisted)
alp.Blockage.BlockageLifetime = partition.Blockage.BlockageLifetime * 4.0;
}
}
}
// reset navigation costs
this.currentTimes = new KeyValuePair<int, Dictionary<ArbiterWaypointId, DownstreamPointOfInterest>>();
}
private TurnBehavior DefaultTurnBehavior(IConnectAreaWaypoints icaw)
{
#region Determine Behavior to Accomplish Turn
// get interconnect
ArbiterInterconnect ai = icaw.ToInterconnect;
// behavior we wish to accomplish
TurnBehavior testTurnBehavior = null;
TurnState testTurnState = null;
#region Turn to Lane
if (ai.FinalGeneric is ArbiterWaypoint)
{
// get final wp
ArbiterWaypoint finalWaypoint = (ArbiterWaypoint)ai.FinalGeneric;
// get turn params
LinePath finalPath;
LineList leftLL;
LineList rightLL;
IntersectionToolkit.TurnInfo(finalWaypoint, out finalPath, out leftLL, out rightLL);
// go into turn
testTurnState = new TurnState(ai, ai.TurnDirection, finalWaypoint.Lane, finalPath, leftLL, rightLL, new ScalarSpeedCommand(2.5));
}
#endregion
#region Turn to Zone
else
{
// final perimeter waypoint
ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)ai.FinalGeneric;
// get turn params
LinePath finalPath;
LineList leftLL;
LineList rightLL;
IntersectionToolkit.ZoneTurnInfo(ai, apw, out finalPath, out leftLL, out rightLL);
// go into turn
testTurnState = new TurnState(ai, ai.TurnDirection, null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(2.5));
}
#endregion
// get behavior
testTurnBehavior = (TurnBehavior)testTurnState.Resume(CoreCommon.Communications.GetVehicleState(), CoreCommon.Communications.GetVehicleSpeed().Value);
testTurnBehavior.TimeStamp = CoreCommon.Communications.GetVehicleState().Timestamp;
// return the behavior
return(testTurnBehavior);
#endregion
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initial"></param>
/// <param name="final"></param>
public ArbiterUserPartition(ArbiterUserPartitionId partitionId, IConnectAreaWaypoints partition, IGenericWaypoint initial, IGenericWaypoint final)
{
this.PartitionId = partitionId;
this.InitialGeneric = initial;
this.FinalGeneric = final;
this.Length = this.InitialGeneric.Position.DistanceTo(this.FinalGeneric.Position);
this.Partition = partition;
this.UserPartitionPath = new LinePath(new Coordinates[] { initial.Position, final.Position });
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initial"></param>
/// <param name="final"></param>
public ArbiterUserPartition(ArbiterUserPartitionId partitionId, IConnectAreaWaypoints partition, IGenericWaypoint initial, IGenericWaypoint final)
{
this.PartitionId = partitionId;
this.InitialGeneric = initial;
this.FinalGeneric = final;
this.Length = this.InitialGeneric.Position.DistanceTo(this.FinalGeneric.Position);
this.Partition = partition;
this.UserPartitionPath = new LinePath(new Coordinates[] { initial.Position, final.Position });
}
/// <summary>
/// Remove blockage from a partition
/// </summary>
/// <param name="partition"></param>
public void RemoveBlockage(IConnectAreaWaypoints partition)
{
partition.Blockage.BlockageExists = false;
partition.Blockage.SecondsSinceObserved = 0.0;
if (partition is ArbiterLanePartition)
{
foreach (ArbiterLanePartition alp in ((ArbiterLanePartition)partition).NonLaneAdjacentPartitions)
{
if (alp.Blockage.BlockageExists && alp.Blockage.BlockageCoordinates.DistanceTo(partition.Blockage.BlockageCoordinates) < 1.0)
{
alp.Blockage.BlockageExists = false;
alp.Blockage.SecondsSinceObserved = 0.0;
}
}
}
}
/// <summary>
/// Checks if everything is clear (stops and priorities over the interconnect) so
/// that we don't have to stop
/// </summary>
/// <param name="ai"></param>
/// <returns></returns>
public bool IsAllClear(IConnectAreaWaypoints ai, VehicleState ourState)
{
// check if intersection clear at the moment
if (this.InsideIntersectionVehicles.Count > 0)
{
return(false);
}
// check all monitors
foreach (IIntersectionQueueable iiq in this.AllMonitors)
{
if (!iiq.IsCompletelyClear(ourState))
{
return(false);
}
}
// clear
return(true);
}
/// <summary>
/// gets nearby stops to the connection
/// </summary>
/// <param name="icaw"></param>
/// <returns></returns>
private List <ArbiterWaypoint> GetNearbyStops(IConnectAreaWaypoints icaw)
{
List <ArbiterWaypoint> aws = new List <ArbiterWaypoint>();
foreach (IArbiterWaypoint iaw in this.roadNetwork.ArbiterWaypoints.Values)
{
if (iaw is ArbiterWaypoint)
{
ArbiterWaypoint aw = (ArbiterWaypoint)iaw;
if (aw.IsStop)
{
if (icaw.DistanceTo(aw.Position) < this.stopLineSearchDist)
{
aws.Add(aw);
}
}
}
}
return(aws);
}
/// <summary>
/// Distance to another connect
/// </summary>
/// <param name="icaw"></param>
/// <returns></returns>
public double DistanceTo(IConnectAreaWaypoints icaw)
{
LinePath.PointOnPath current = this.PartitionPath.StartPoint;
double inc = 1.0;
double dist = 0;
double minDist = double.MaxValue;
while (dist == 0)
{
double tmpDist = icaw.DistanceTo(this.PartitionPath.GetPoint(current));
if (tmpDist < minDist)
{
minDist = tmpDist;
}
dist = inc;
current = PartitionPath.AdvancePoint(current, ref dist);
}
return(minDist);
}
/// <summary>
/// get nearby
/// </summary>
/// <param name="icaw"></param>
/// <param name="icaws"></param>
/// <returns></returns>
private List <IConnectAreaWaypoints> GetNearbyPartitions(IConnectAreaWaypoints icaw, List <IConnectAreaWaypoints> icaws)
{
List <IConnectAreaWaypoints> final = new List <IConnectAreaWaypoints>();
foreach (IConnectAreaWaypoints tmp in icaws)
{
double dist = this.nearbyPartitionsDist;
if (icaw is SceneZonePartition)
{
dist = ((SceneZonePartition)icaw).Zone.Perimeter.PerimeterPolygon.CalculateBoundingCircle().r + this.nearbyPartitionsDist;
}
else if (tmp is SceneZonePartition)
{
dist = ((SceneZonePartition)tmp).Zone.Perimeter.PerimeterPolygon.CalculateBoundingCircle().r + this.nearbyPartitionsDist;
}
if (icaw.DistanceTo(tmp) <= dist)
{
final.Add(tmp);
}
}
return(final);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="ourExit"></param>
public IntersectionMonitor(ITraversableWaypoint ourExit, ArbiterIntersection ai, VehicleState ourState, IConnectAreaWaypoints desired)
{
// set intersection
this.Intersection = ai;
// create monitors
this.AllMonitors = new List <IIntersectionQueueable>();
this.PriorityMonitors = new List <IIntersectionQueueable>();
this.IntersectionPriorityQueue = new List <IIntersectionQueueable>();
this.InsideIntersectionVehicles = new List <IntersectionVehicleMonitor>();
this.NonStopPriorityAreas = new List <IDominantMonitor>();
this.PreviouslyWaiting = new List <IDominantMonitor>();
this.AllMonitors = new List <IIntersectionQueueable>();
this.cooldownTimer = new Stopwatch();
#region Stopped Exits
// get ours
IIntersectionQueueable ourMonitor = null;
// get stopped exits
foreach (ArbiterStoppedExit ase in ai.StoppedExits)
{
// add a monitor
SubmissiveEntryMonitor sem = new SubmissiveEntryMonitor(this, ase.Waypoint, ase.Waypoint.Equals(ourExit));
// add to monitors
PriorityMonitors.Add(sem);
// check not our
if (!sem.IsOurs)
{
// initial update
sem.Update(ourState);
// check if add first
if (!sem.Clear(ourState))
{
IntersectionPriorityQueue.Add(sem);
}
}
else
{
// set ours
ourMonitor = sem;
this.OurMonitor = ourMonitor;
}
}
// check if our monitor exists
if (ourMonitor != null)
{
// add ours
this.IntersectionPriorityQueue.Add(ourMonitor);
}
#endregion
#region Priority Areas Over Interconnect
// check contains priority lane for this
if (ai.PriorityLanes.ContainsKey(desired.ToInterconnect))
{
// loop through all other priority monitors over this interconnect
foreach (IntersectionInvolved ii in ai.PriorityLanes[desired.ToInterconnect])
{
// check area type if lane
if (ii.Area is ArbiterLane)
{
// add lane to non stop priority areas
this.NonStopPriorityAreas.Add(new DominantLaneEntryMonitor(this, ii));
}
// otherwise is zone
else if (ii.Area is ArbiterZone)
{
// add lane to non stop priority areas
this.NonStopPriorityAreas.Add(
new DominantZoneEntryMonitor((ArbiterPerimeterWaypoint)ii.Exit, ((ArbiterInterconnect)desired), false, this, ii));
}
else
{
throw new ArgumentException("unknown intersection involved area", "ii");
}
}
}
// otherwise be like, wtf?
else
{
ArbiterOutput.Output("Exception: intersection: " + this.Intersection.ToString() + " priority lanes does not contain interconnect: " + desired.ToString() + " returning can go");
//ArbiterOutput.Output("Error in intersection monitor!!!!!!!!!@Q!!, desired interconnect: " + desired.ToInterconnect.ToString() + " not found in intersection: " + ai.ToString() + ", not setting any dominant monitors");
}
#endregion
#region Entry Area
if (desired.FinalGeneric is ArbiterWaypoint)
{
this.EntryAreaMonitor = new LaneEntryAreaMonitor((ArbiterWaypoint)desired.FinalGeneric);
}
else if (desired.FinalGeneric is ArbiterPerimeterWaypoint)
{
this.EntryAreaMonitor = new ZoneAreaEntryMonitor((ArbiterPerimeterWaypoint)desired.FinalGeneric, (ArbiterInterconnect)desired,
false, this, new IntersectionInvolved(ourExit, ((ArbiterPerimeterWaypoint)desired.FinalGeneric).Perimeter.Zone,
((ArbiterInterconnect)desired).TurnDirection));
}
else
{
throw new Exception("unhandled desired interconnect final type");
}
#endregion
#region Our Monitor
// check ours
if (ourExit is ArbiterWaypoint)
{
this.OurMonitor = new DominantLaneEntryMonitor(this,
new IntersectionInvolved(ourExit, ((ArbiterWaypoint)ourExit).Lane,
desired is ArbiterInterconnect ? ((ArbiterInterconnect)desired).TurnDirection : ArbiterTurnDirection.Straight));
}
else if (ourExit is ArbiterPerimeterWaypoint)
{
// add lane to non stop priority areas
this.OurMonitor =
new DominantZoneEntryMonitor((ArbiterPerimeterWaypoint)desired.InitialGeneric, ((ArbiterInterconnect)desired), true, this,
new IntersectionInvolved(ourExit, ((ArbiterPerimeterWaypoint)desired.InitialGeneric).Perimeter.Zone,
((ArbiterInterconnect)desired).TurnDirection));
}
#endregion
// add to all
this.AllMonitors.AddRange(this.PriorityMonitors);
foreach (IIntersectionQueueable iiq in this.NonStopPriorityAreas)
{
this.AllMonitors.Add(iiq);
}
this.AllMonitors.Add(this.EntryAreaMonitor);
// update all
this.Update(ourState);
// check if we need to populate previously waiting
if (this.OurMonitor is IDominantMonitor)
{
// cast our
IDominantMonitor ours = (IDominantMonitor)this.OurMonitor;
// loop through to determine those previously waiting
foreach (IDominantMonitor idm in this.NonStopPriorityAreas)
{
if (idm.Waypoint != null && !idm.Waypoint.IsStop && idm.WaitingTimerRunning)
{
this.PreviouslyWaiting.Add(idm);
}
}
}
}
/// <summary>
/// Distance to another connect
/// </summary>
/// <param name="icaw"></param>
/// <returns></returns>
public double DistanceTo(IConnectAreaWaypoints icaw)
{
LinePath.PointOnPath current = this.PartitionPath.StartPoint;
double inc = 1.0;
double dist = 0;
double minDist = double.MaxValue;
while (dist == 0)
{
double tmpDist = icaw.DistanceTo(this.PartitionPath.GetPoint(current));
if (tmpDist < minDist)
minDist = tmpDist;
dist = inc;
current = PartitionPath.AdvancePoint(current, ref dist);
}
return minDist;
}
/// <summary>
/// Set blockage time cost for the partition
/// </summary>
/// <param name="partition"></param>
/// <remarks>Should set interconnect blockage to be initial lane partition of next segment</remarks>
public void SetBlockageCost(IConnectAreaWaypoints partition)
{
// default time
partition.Blockage.BlockageTimeCost = 1800;
#region Old
/*
*
#region Lane Partition
*
* // check if segment partition
* if (partition is ArbiterLanePartition)
* {
* // get partition
* ArbiterLanePartition alp = (ArbiterLanePartition)partition;
*
* // make sure the other way is valid
* bool otherValid = false;
*
* // check 1
* if (alp.Lane.Way.WayId.Number == 1)
* {
* if (alp.Lane.Way.Segment.Way2.IsValid)
* otherValid = true;
* }
*
* // check 2
* if (alp.Lane.Way.WayId.Number == 2)
* {
* if (alp.Lane.Way.Segment.Way1.IsValid)
* otherValid = true;
* }
*
* // check other way valid
* if (otherValid)
* {
* // determine adjacent partitions
* List<NavigableEdge> toRemove = this.AdjacentPartitions(alp);
*
* // get final waypoint of partition in other segment closest to partition's initial waypoint to plan from
* ArbiterWay otherWay = null;
*
* // check 1
* if (alp.Lane.Way.WayId.Number == 1)
* otherWay = alp.Lane.Way.Segment.Way2;
*
* // check 2
* if (alp.Lane.Way.WayId.Number == 2)
* otherWay = alp.Lane.Way.Segment.Way1;
*
* // get lanes of other way
* ArbiterLane al = null;
* foreach (ArbiterLane alTmp in otherWay.Lanes.Values)
* {
* al = alTmp;
* }
*
* // get closest waypoint
* ArbiterWaypoint awOther = al.GetClosestPartition(alp.Initial.Position).Final;
*
* // get replan point
* List<DownstreamPointOfInterest> dpoi = this.Downstream(awOther.PreviousPartition.Initial.Position + awOther.PreviousPartition.Vector().Normalize(awOther.PreviousPartition.Length / 2.0), awOther.Lane.Way, true, new List<ArbiterWaypoint>());
* ArbiterWaypoint nextOtherExit = dpoi != null && dpoi.Count > 0 ? dpoi[0].PointOfInterest : null;
*
* // get next entry into lane
* ArbiterWaypoint nextEntry = this.NextEntry(alp.Final);
*
* if (nextEntry == null || nextOtherExit == null)
* {
* // default time
* partition.Blockage.BlockageTimeCost = 10000;
* }
* else
* {
* // plan and set time
* aStar planner = new aStar(nextOtherExit, nextEntry, toRemove);
* planner.Plan();
* partition.Blockage.BlockageTimeCost = planner.TotalTime;
* }
* }
* else
* {
* // default time
* partition.Blockage.BlockageTimeCost = 10000;
* }
* }
*
#endregion
*
*/
#endregion
}
/// <summary>
/// gets nearby stops to the connection
/// </summary>
/// <param name="icaw"></param>
/// <returns></returns>
private List<ArbiterWaypoint> GetNearbyStops(IConnectAreaWaypoints icaw)
{
List<ArbiterWaypoint> aws = new List<ArbiterWaypoint>();
foreach (IArbiterWaypoint iaw in this.roadNetwork.ArbiterWaypoints.Values)
{
if (iaw is ArbiterWaypoint)
{
ArbiterWaypoint aw = (ArbiterWaypoint)iaw;
if (aw.IsStop)
{
if (icaw.DistanceTo(aw.Position) < this.stopLineSearchDist)
{
aws.Add(aw);
}
}
}
}
return aws;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="position"></param>
/// <param name="waypointId"></param>
/// <param name="partition"></param>
public ArbiterUserWaypoint(Coordinates position, ArbiterUserWaypointId waypointId, IConnectAreaWaypoints partition)
{
this.position = position;
this.WaypointId = waypointId;
this.Partition = partition;
}
private TurnBehavior DefaultTurnBehavior(IConnectAreaWaypoints icaw)
{
#region Determine Behavior to Accomplish Turn
// get interconnect
ArbiterInterconnect ai = icaw.ToInterconnect;
// behavior we wish to accomplish
TurnBehavior testTurnBehavior = null;
TurnState testTurnState = null;
#region Turn to Lane
if (ai.FinalGeneric is ArbiterWaypoint)
{
// get final wp
ArbiterWaypoint finalWaypoint = (ArbiterWaypoint)ai.FinalGeneric;
// get turn params
LinePath finalPath;
LineList leftLL;
LineList rightLL;
IntersectionToolkit.TurnInfo(finalWaypoint, out finalPath, out leftLL, out rightLL);
// go into turn
testTurnState = new TurnState(ai, ai.TurnDirection, finalWaypoint.Lane, finalPath, leftLL, rightLL, new ScalarSpeedCommand(2.5));
}
#endregion
#region Turn to Zone
else
{
// final perimeter waypoint
ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)ai.FinalGeneric;
// get turn params
LinePath finalPath;
LineList leftLL;
LineList rightLL;
IntersectionToolkit.ZoneTurnInfo(ai, apw, out finalPath, out leftLL, out rightLL);
// go into turn
testTurnState = new TurnState(ai, ai.TurnDirection, null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(2.5));
}
#endregion
// get behavior
testTurnBehavior = (TurnBehavior)testTurnState.Resume(CoreCommon.Communications.GetVehicleState(), CoreCommon.Communications.GetVehicleSpeed().Value);
testTurnBehavior.TimeStamp = CoreCommon.Communications.GetVehicleState().Timestamp;
// return the behavior
return testTurnBehavior;
#endregion
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="position"></param>
/// <param name="waypointId"></param>
/// <param name="partition"></param>
public ArbiterUserWaypoint(Coordinates position, ArbiterUserWaypointId waypointId, IConnectAreaWaypoints partition)
{
this.position = position;
this.WaypointId = waypointId;
this.Partition = partition;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="ourExit"></param>
public IntersectionMonitor(ITraversableWaypoint ourExit, ArbiterIntersection ai, VehicleState ourState, IConnectAreaWaypoints desired)
{
// set intersection
this.Intersection = ai;
// create monitors
this.AllMonitors = new List<IIntersectionQueueable>();
this.PriorityMonitors = new List<IIntersectionQueueable>();
this.IntersectionPriorityQueue = new List<IIntersectionQueueable>();
this.InsideIntersectionVehicles = new List<IntersectionVehicleMonitor>();
this.NonStopPriorityAreas = new List<IDominantMonitor>();
this.PreviouslyWaiting = new List<IDominantMonitor>();
this.AllMonitors = new List<IIntersectionQueueable>();
this.cooldownTimer = new Stopwatch();
#region Stopped Exits
// get ours
IIntersectionQueueable ourMonitor = null;
// get stopped exits
foreach(ArbiterStoppedExit ase in ai.StoppedExits)
{
// add a monitor
SubmissiveEntryMonitor sem = new SubmissiveEntryMonitor(this, ase.Waypoint, ase.Waypoint.Equals(ourExit));
// add to monitors
PriorityMonitors.Add(sem);
// check not our
if(!sem.IsOurs)
{
// initial update
sem.Update(ourState);
// check if add first
if(!sem.Clear(ourState))
IntersectionPriorityQueue.Add(sem);
}
else
{
// set ours
ourMonitor = sem;
this.OurMonitor = ourMonitor;
}
}
// check if our monitor exists
if (ourMonitor != null)
{
// add ours
this.IntersectionPriorityQueue.Add(ourMonitor);
}
#endregion
#region Priority Areas Over Interconnect
// check contains priority lane for this
if (ai.PriorityLanes.ContainsKey(desired.ToInterconnect))
{
// loop through all other priority monitors over this interconnect
foreach (IntersectionInvolved ii in ai.PriorityLanes[desired.ToInterconnect])
{
// check area type if lane
if (ii.Area is ArbiterLane)
{
// add lane to non stop priority areas
this.NonStopPriorityAreas.Add(new DominantLaneEntryMonitor(this, ii));
}
// otherwise is zone
else if (ii.Area is ArbiterZone)
{
// add lane to non stop priority areas
this.NonStopPriorityAreas.Add(
new DominantZoneEntryMonitor((ArbiterPerimeterWaypoint)ii.Exit, ((ArbiterInterconnect)desired), false, this, ii));
}
else
{
throw new ArgumentException("unknown intersection involved area", "ii");
}
}
}
// otherwise be like, wtf?
else
{
ArbiterOutput.Output("Exception: intersection: " + this.Intersection.ToString() + " priority lanes does not contain interconnect: " + desired.ToString() + " returning can go");
//ArbiterOutput.Output("Error in intersection monitor!!!!!!!!!@Q!!, desired interconnect: " + desired.ToInterconnect.ToString() + " not found in intersection: " + ai.ToString() + ", not setting any dominant monitors");
}
#endregion
#region Entry Area
if (desired.FinalGeneric is ArbiterWaypoint)
{
this.EntryAreaMonitor = new LaneEntryAreaMonitor((ArbiterWaypoint)desired.FinalGeneric);
}
else if (desired.FinalGeneric is ArbiterPerimeterWaypoint)
{
this.EntryAreaMonitor = new ZoneAreaEntryMonitor((ArbiterPerimeterWaypoint)desired.FinalGeneric, (ArbiterInterconnect)desired,
false, this, new IntersectionInvolved(ourExit, ((ArbiterPerimeterWaypoint)desired.FinalGeneric).Perimeter.Zone,
((ArbiterInterconnect)desired).TurnDirection));
}
else
{
throw new Exception("unhandled desired interconnect final type");
}
#endregion
#region Our Monitor
// check ours
if (ourExit is ArbiterWaypoint)
{
this.OurMonitor = new DominantLaneEntryMonitor(this,
new IntersectionInvolved(ourExit, ((ArbiterWaypoint)ourExit).Lane,
desired is ArbiterInterconnect ? ((ArbiterInterconnect)desired).TurnDirection : ArbiterTurnDirection.Straight));
}
else if (ourExit is ArbiterPerimeterWaypoint)
{
// add lane to non stop priority areas
this.OurMonitor =
new DominantZoneEntryMonitor((ArbiterPerimeterWaypoint)desired.InitialGeneric, ((ArbiterInterconnect)desired), true, this,
new IntersectionInvolved(ourExit, ((ArbiterPerimeterWaypoint)desired.InitialGeneric).Perimeter.Zone,
((ArbiterInterconnect)desired).TurnDirection));
}
#endregion
// add to all
this.AllMonitors.AddRange(this.PriorityMonitors);
foreach (IIntersectionQueueable iiq in this.NonStopPriorityAreas)
this.AllMonitors.Add(iiq);
this.AllMonitors.Add(this.EntryAreaMonitor);
// update all
this.Update(ourState);
// check if we need to populate previously waiting
if(this.OurMonitor is IDominantMonitor)
{
// cast our
IDominantMonitor ours = (IDominantMonitor)this.OurMonitor;
// loop through to determine those previously waiting
foreach (IDominantMonitor idm in this.NonStopPriorityAreas)
{
if(idm.Waypoint != null && !idm.Waypoint.IsStop && idm.WaitingTimerRunning)
this.PreviouslyWaiting.Add(idm);
}
}
}
public double DistanceTo(IConnectAreaWaypoints icaw)
{
return icaw.DistanceTo(this.Zone.Perimeter.PerimeterPolygon.Center);
}
/// <summary>
/// Write partition informaton
/// </summary>
/// <param name="sw"></param>
private void WritePartitionInformation(StreamWriter sw)
{
// get list of all partitions that connect waypoints
List <IConnectAreaWaypoints> icaws = new List <IConnectAreaWaypoints>();
#region Populate partitions
// get lane partitions
foreach (ArbiterSegment asg in roadNetwork.ArbiterSegments.Values)
{
foreach (ArbiterLane al in asg.Lanes.Values)
{
foreach (ArbiterLanePartition alp in al.Partitions)
{
icaws.Add(alp);
}
}
}
// get interconnects
foreach (ArbiterInterconnect ai in roadNetwork.ArbiterInterconnects.Values)
{
icaws.Add(ai);
}
// zones (holy stuff what a hack)
foreach (ArbiterZone az in roadNetwork.ArbiterZones.Values)
{
icaws.Add(new SceneZonePartition(az));
}
#endregion
// notify
sw.WriteLine("NumberOfPartitions" + "\t" + icaws.Count.ToString());
string completionPercent = "";
#region Create Partitions in Road Graph
// write each
for (int i = 0; i < icaws.Count; i++)
{
IConnectAreaWaypoints icaw = icaws[i];
sw.WriteLine("Partition");
string id = "";
if (icaw is SceneZonePartition)
{
id = ("PartitionId" + "\t" + ((SceneZonePartition)icaw).Zone.ToString());
}
else
{
id = ("PartitionId" + "\t" + icaw.ConnectionId.ToString());
}
sw.WriteLine(id);
// notify
double percent = ((double)i) / ((double)icaws.Count) * 100.0;
string tmpP = percent.ToString("F0") + "% Complete";
if (tmpP != completionPercent)
{
completionPercent = tmpP;
EditorOutput.WriteLine(completionPercent);
}
#region Interconnect
if (icaw is ArbiterInterconnect)
{
ArbiterInterconnect ai = (ArbiterInterconnect)icaw;
sw.WriteLine("PartitionType" + "\t" + "Interconnect");
sw.WriteLine("Sparse" + "\t" + "False");
sw.WriteLine("FitType" + "\t" + "Line");
Coordinates c = ai.FinalGeneric.Position - ai.InitialGeneric.Position;
sw.WriteLine("FitParameters" + "\t" + c.ArcTan.ToString("F6"));
sw.WriteLine("LeftBoundary" + "\t" + "None");
sw.WriteLine("RightBoundary" + "\t" + "None");
sw.WriteLine("NumberOfPoints" + "\t" + "2");
sw.WriteLine("Points");
sw.WriteLine(ai.InitialGeneric.ToString());
sw.WriteLine(ai.FinalGeneric.ToString());
sw.WriteLine("End_Points");
List <ArbiterWaypoint> aws = this.GetNearbyStops(ai);
sw.WriteLine("NumberOfNearbyStoplines" + "\t" + aws.Count);
if (aws.Count != 0)
{
sw.WriteLine("NearbyStoplines");
foreach (ArbiterWaypoint aw in aws)
{
sw.WriteLine(aw.ToString());
}
sw.WriteLine("End_NearbyStoplines");
}
#region Adjacent
List <string> adjacentPartitions = new List <string>();
// add current
adjacentPartitions.Add(ai.ToString());
#region Initial
if (icaw.InitialGeneric is ArbiterWaypoint)
{
// wp
ArbiterWaypoint aw = (ArbiterWaypoint)icaw.InitialGeneric;
// prev
if (aw.PreviousPartition != null)
{
adjacentPartitions.Add(aw.PreviousPartition.ToString());
}
// next
if (aw.NextPartition != null)
{
adjacentPartitions.Add(aw.NextPartition.ToString());
}
// exits
if (aw.IsExit)
{
foreach (ArbiterInterconnect ais in aw.Exits)
{
if (!ais.Equals(ai))
{
adjacentPartitions.Add(ais.ToString());
}
}
}
if (aw.IsEntry)
{
foreach (ArbiterInterconnect ais in aw.Entries)
{
if (!ais.Equals(ai))
{
adjacentPartitions.Add(ais.ToString());
}
}
}
}
else if (icaw.InitialGeneric is ArbiterPerimeterWaypoint)
{
adjacentPartitions.Add((new SceneZonePartition(((ArbiterPerimeterWaypoint)icaw.InitialGeneric).Perimeter.Zone)).ToString());
}
#endregion
#region Final
if (icaw.FinalGeneric is ArbiterWaypoint)
{
// wp
ArbiterWaypoint aw = (ArbiterWaypoint)icaw.FinalGeneric;
// prev
if (aw.PreviousPartition != null)
{
adjacentPartitions.Add(aw.PreviousPartition.ToString());
}
// next
if (aw.NextPartition != null)
{
adjacentPartitions.Add(aw.NextPartition.ToString());
}
// exits
if (aw.IsExit)
{
foreach (ArbiterInterconnect ais in aw.Exits)
{
adjacentPartitions.Add(ais.ToString());
}
}
if (aw.IsEntry)
{
foreach (ArbiterInterconnect ais in aw.Entries)
{
if (!ais.Equals(ai))
{
adjacentPartitions.Add(ais.ToString());
}
}
}
}
else if (icaw.FinalGeneric is ArbiterPerimeterWaypoint)
{
adjacentPartitions.Add((new SceneZonePartition(((ArbiterPerimeterWaypoint)icaw.FinalGeneric).Perimeter.Zone)).ToString());
}
#endregion
sw.WriteLine("NumberOfLaneAdjacentPartitions" + "\t" + adjacentPartitions.Count.ToString());
if (adjacentPartitions.Count != 0)
{
sw.WriteLine("LaneAdjacentPartitions");
foreach (string s in adjacentPartitions)
{
sw.WriteLine(s);
}
sw.WriteLine("End_LaneAdjacentPartitions");
}
#endregion
sw.WriteLine("NumberOfLeftLaneAdjacentPartitions" + "\t" + "0");
sw.WriteLine("NumberOfRightLaneAdjacentPartitions" + "\t" + "0");
List <IConnectAreaWaypoints> nearby = this.GetNearbyPartitions(ai, icaws);
sw.WriteLine("NumberOfNearbyPartitions" + "\t" + nearby.Count.ToString());
if (nearby.Count != 0)
{
sw.WriteLine("NearbyPartitions");
foreach (IConnectAreaWaypoints tmp in nearby)
{
sw.WriteLine(tmp.ToString());
}
sw.WriteLine("End_NearbyPartitions");
}
sw.WriteLine("End_Partition");
}
#endregion
#region Zone
else if (icaw is SceneZonePartition)
{
SceneZonePartition szp = (SceneZonePartition)icaw;
sw.WriteLine("PartitionType" + "\t" + "Zone");
sw.WriteLine("Sparse" + "\t" + "False");
sw.WriteLine("FitType" + "\t" + "Polygon");
string count = szp.Zone.Perimeter.PerimeterPoints.Count.ToString();
string wps = "";
foreach (ArbiterPerimeterWaypoint apw in szp.Zone.Perimeter.PerimeterPoints.Values)
{
wps = wps + "\t" + apw.Position.X.ToString("f6") + "\t" + apw.Position.Y.ToString("f6");
}
sw.WriteLine("FitParameters" + "\t" + count + wps);
sw.WriteLine("LeftBoundary" + "\t" + "None");
sw.WriteLine("RightBoundary" + "\t" + "None");
sw.WriteLine("NumberOfPoints" + "\t" + szp.Zone.Perimeter.PerimeterPoints.Count.ToString());
sw.WriteLine("Points");
foreach (ArbiterPerimeterWaypoint apw in szp.Zone.Perimeter.PerimeterPoints.Values)
{
sw.WriteLine(apw.WaypointId.ToString());
}
sw.WriteLine("End_Points");
List <ArbiterWaypoint> aws = this.GetNearbyStops(szp);
sw.WriteLine("NumberOfNearbyStoplines" + "\t" + aws.Count);
if (aws.Count != 0)
{
sw.WriteLine("NearbyStoplines");
foreach (ArbiterWaypoint aw in aws)
{
sw.WriteLine(aw.ToString());
}
sw.WriteLine("End_NearbyStoplines");
}
#region Adjacent
List <string> adjacentStrings = new List <string>();
// add current
adjacentStrings.Add(szp.ToString());
foreach (ArbiterPerimeterWaypoint apw in szp.Zone.Perimeter.PerimeterPoints.Values)
{
if (apw.IsExit)
{
foreach (ArbiterInterconnect ai in apw.Exits)
{
adjacentStrings.Add(ai.ToString());
}
}
if (apw.IsEntry)
{
foreach (ArbiterInterconnect ais in apw.Entries)
{
adjacentStrings.Add(ais.ToString());
}
}
}
sw.WriteLine("NumberOfLaneAdjacentPartitions" + "\t" + adjacentStrings.Count.ToString());
if (adjacentStrings.Count != 0)
{
sw.WriteLine("LaneAdjacentPartitions");
foreach (string s in adjacentStrings)
{
sw.WriteLine(s);
}
sw.WriteLine("End_LaneAdjacentPartitions");
}
#endregion
sw.WriteLine("NumberOfLeftLaneAdjacentPartitions" + "\t" + "0");
sw.WriteLine("NumberOfRightLaneAdjacentPartitions" + "\t" + "0");
List <IConnectAreaWaypoints> nearby = this.GetNearbyPartitions(szp, icaws);
sw.WriteLine("NumberOfNearbyPartitions" + "\t" + nearby.Count.ToString());
if (nearby.Count != 0)
{
sw.WriteLine("NearbyPartitions");
foreach (IConnectAreaWaypoints tmp in nearby)
{
sw.WriteLine(tmp.ToString());
}
sw.WriteLine("End_NearbyPartitions");
}
sw.WriteLine("End_Partition");
}
#endregion
#region Lane
else if (icaw is ArbiterLanePartition)
{
ArbiterLanePartition alp = (ArbiterLanePartition)icaw;
sw.WriteLine("PartitionType" + "\t" + "Lane");
string sparseString = alp.Type == PartitionType.Sparse ? "True" : "False";
sw.WriteLine("Sparse" + "\t" + sparseString);
if (alp.Type != PartitionType.Sparse) //alp.UserPartitions.Count <= 1)
{
sw.WriteLine("FitType" + "\t" + "Line");
sw.WriteLine("FitParameters" + "\t" + alp.Vector().ArcTan.ToString("F6"));
}
else
{
sw.WriteLine("FitType" + "\t" + "Polygon");
/*List<Coordinates> polyCoords = new List<Coordinates>();
* polyCoords.Add(alp.Initial.Position);
* polyCoords.AddRange(alp.NotInitialPathCoords());
* LinePath lpr = (new LinePath(polyCoords)).ShiftLateral(-TahoeParams.VL * 3.0);
* LinePath lpl = (new LinePath(polyCoords)).ShiftLateral(TahoeParams.VL * 3.0);
* List<Coordinates> finalCoords = new List<Coordinates>(polyCoords.ToArray());
* finalCoords.AddRange(lpr);
* finalCoords.AddRange(lpl);
* Polygon p = Polygon.GrahamScan(finalCoords);*/
if (alp.SparsePolygon == null)
{
alp.SetDefaultSparsePolygon();
}
string coordinateString = "";
foreach (Coordinates c in alp.SparsePolygon)
{
coordinateString = coordinateString + "\t" + c.X.ToString("F6") + "\t" + c.Y.ToString("F6");
}
sw.WriteLine("FitParameters" + "\t" + alp.SparsePolygon.Count.ToString() + coordinateString);
}
sw.WriteLine("LaneWidth" + "\t" + alp.Lane.Width.ToString("F6"));
sw.WriteLine("LeftBoundary" + "\t" + alp.Lane.BoundaryLeft.ToString());
sw.WriteLine("RightBoundary" + "\t" + alp.Lane.BoundaryRight.ToString());
sw.WriteLine("NumberOfPoints" + "\t" + "2");
sw.WriteLine("Points");
sw.WriteLine(alp.InitialGeneric.ToString());
sw.WriteLine(alp.FinalGeneric.ToString());
sw.WriteLine("End_Points");
List <ArbiterWaypoint> aws = this.GetNearbyStops(alp);
sw.WriteLine("NumberOfNearbyStoplines" + "\t" + aws.Count);
if (aws.Count != 0)
{
sw.WriteLine("NearbyStoplines");
foreach (ArbiterWaypoint aw in aws)
{
sw.WriteLine(aw.ToString());
}
sw.WriteLine("End_NearbyStoplines");
}
#region Adjacent
List <string> adjacentPartitions = new List <string>();
// add current
adjacentPartitions.Add(alp.ToString());
#region Initial
if (icaw.InitialGeneric is ArbiterWaypoint)
{
// wp
ArbiterWaypoint aw = (ArbiterWaypoint)icaw.InitialGeneric;
// prev
if (aw.PreviousPartition != null)
{
adjacentPartitions.Add(aw.PreviousPartition.ToString());
}
// next
if (aw.NextPartition != null && !aw.NextPartition.Equals(alp))
{
adjacentPartitions.Add(aw.NextPartition.ToString());
}
// exits
if (aw.IsExit)
{
foreach (ArbiterInterconnect ais in aw.Exits)
{
adjacentPartitions.Add(ais.ToString());
}
}
if (aw.IsEntry)
{
foreach (ArbiterInterconnect ais in aw.Entries)
{
adjacentPartitions.Add(ais.ToString());
}
}
}
#endregion
#region Final
if (icaw.FinalGeneric is ArbiterWaypoint)
{
// wp
ArbiterWaypoint aw = (ArbiterWaypoint)icaw.FinalGeneric;
// prev
if (aw.PreviousPartition != null && !aw.PreviousPartition.Equals(alp))
{
adjacentPartitions.Add(aw.PreviousPartition.ToString());
}
// next
if (aw.NextPartition != null)
{
adjacentPartitions.Add(aw.NextPartition.ToString());
}
// exits
if (aw.IsExit)
{
foreach (ArbiterInterconnect ais in aw.Exits)
{
adjacentPartitions.Add(ais.ToString());
}
}
if (aw.IsEntry)
{
foreach (ArbiterInterconnect ais in aw.Entries)
{
adjacentPartitions.Add(ais.ToString());
}
}
}
#endregion
sw.WriteLine("NumberOfLaneAdjacentPartitions" + "\t" + adjacentPartitions.Count.ToString());
if (adjacentPartitions.Count != 0)
{
sw.WriteLine("LaneAdjacentPartitions");
foreach (string s in adjacentPartitions)
{
sw.WriteLine(s);
}
sw.WriteLine("End_LaneAdjacentPartitions");
}
#endregion
List <string> leftAlps = new List <string>();
List <string> rightAlps = new List <string>();
foreach (ArbiterLanePartition tmpAlp in alp.NonLaneAdjacentPartitions)
{
if (tmpAlp.Lane.Equals(alp.Lane.LaneOnLeft))
{
leftAlps.Add(tmpAlp.ToString());
}
else
{
rightAlps.Add(tmpAlp.ToString());
}
}
sw.WriteLine("NumberOfLeftLaneAdjacentPartitions" + "\t" + leftAlps.Count.ToString());
if (leftAlps.Count != 0)
{
sw.WriteLine("LeftLaneAdjacentPartitions");
foreach (string s in leftAlps)
{
sw.WriteLine(s);
}
sw.WriteLine("End_LeftLaneAdjacentPartitions");
}
sw.WriteLine("NumberOfRightLaneAdjacentPartitions" + "\t" + rightAlps.Count.ToString());
if (rightAlps.Count != 0)
{
sw.WriteLine("RightLaneAdjacentPartitions");
foreach (string s in rightAlps)
{
sw.WriteLine(s);
}
sw.WriteLine("End_RightLaneAdjacentPartitions");
}
List <IConnectAreaWaypoints> nearby = this.GetNearbyPartitions(alp, icaws);
sw.WriteLine("NumberOfNearbyPartitions" + "\t" + nearby.Count.ToString());
if (nearby.Count != 0)
{
sw.WriteLine("NearbyPartitions");
foreach (IConnectAreaWaypoints tmp in nearby)
{
sw.WriteLine(tmp.ToString());
}
sw.WriteLine("End_NearbyPartitions");
}
sw.WriteLine("End_Partition");
}
#endregion
}
#endregion
}
/// <summary>
/// get nearby
/// </summary>
/// <param name="icaw"></param>
/// <param name="icaws"></param>
/// <returns></returns>
private List<IConnectAreaWaypoints> GetNearbyPartitions(IConnectAreaWaypoints icaw, List<IConnectAreaWaypoints> icaws)
{
List<IConnectAreaWaypoints> final = new List<IConnectAreaWaypoints>();
foreach (IConnectAreaWaypoints tmp in icaws)
{
double dist = this.nearbyPartitionsDist;
if (icaw is SceneZonePartition)
dist = ((SceneZonePartition)icaw).Zone.Perimeter.PerimeterPolygon.CalculateBoundingCircle().r + this.nearbyPartitionsDist;
else if(tmp is SceneZonePartition)
dist = ((SceneZonePartition)tmp).Zone.Perimeter.PerimeterPolygon.CalculateBoundingCircle().r + this.nearbyPartitionsDist;
if (icaw.DistanceTo(tmp) <= dist)
{
final.Add(tmp);
}
}
return final;
}
/// <summary>
/// Checks if it is available for us to traverse the interconnect passed in
/// </summary>
/// <param name="ai"></param>
/// <returns></returns>
public bool CanTraverse(IConnectAreaWaypoints ai, VehicleState ourState)
{
// if we can go
bool?canGo = null;
// check for bug where interconnect not in intersection
if (!this.Intersection.PriorityLanes.ContainsKey(ai.ToInterconnect))
{
ArbiterOutput.Output("Exception: intersection: " + this.Intersection.ToString() + " priority lanes does not contain interconnect: " + ai.ToString() + " returning can go");
//ArbiterOutput.Output("Exception: intersection: " + this.Intersection.ToString() + " priority lanes does not contain interconnect: " + ai.ToString() + " returning can go");
//return true;
}
// check entry clear
if (!this.EntryAreaMonitor.Clear(ourState))
{
canGo = false;
}
// Check if we have priority if we're a stop
if (ai.InitialGeneric is ArbiterWaypoint && ((ArbiterWaypoint)ai.InitialGeneric).IsStop && IntersectionPriorityQueue[0].IsOurs)
{
// check if intersection clear at the moment
foreach (IntersectionVehicleMonitor ivm in this.InsideIntersectionVehicles)
{
if (!ivm.Failed(true))
{
canGo = false;
}
}
// check all priorities over us if clear
foreach (IIntersectionQueueable iiq in this.NonStopPriorityAreas)
{
if (!iiq.Clear(ourState))
{
canGo = false;
}
}
// clear
if (!canGo.HasValue)
{
canGo = true;
}
}
// check if not a stop if clear
else if (ai.InitialGeneric is ITraversableWaypoint && !((ITraversableWaypoint)ai.InitialGeneric).IsStop)
{
// get our monitor
IDominantMonitor ours = (IDominantMonitor)this.OurMonitor;
// check if intersection clear at the moment
foreach (IntersectionVehicleMonitor ivm in this.InsideIntersectionVehicles)
{
if (!ivm.Failed(true))
{
canGo = false;
}
}
// check all priorities over us if clear
foreach (IDominantMonitor iiq in this.NonStopPriorityAreas)
{
// checks if clear by default
bool isClear = iiq.Clear(ourState);
if (!isClear && !iiq.Waiting)
{
canGo = false;
}
if (iiq.Waiting)
{
ArbiterOutput.Output("DLEM: " + iiq.Area.ToString() + " vehicle waiting, hence clear");
}
}
// quick check
if (canGo.HasValue && this.PreviouslyWaiting.Count > 0)
{
canGo = false;
}
else
{
// check if previously waiting doesn't exist
foreach (IDominantMonitor idm in this.PreviouslyWaiting)
{
// check if waiting for this person excessively
if (!idm.ExcessiveWaiting)
{
canGo = false;
}
}
}
// clear
if (!canGo.HasValue)
{
canGo = true;
}
}
// nope, can't go if fall through
if (!canGo.HasValue)
{
canGo = false;
}
#region Cooldown
// final boolean if we can go
bool finalGo = canGo.Value;
if (finalGo)
{
ArbiterOutput.Output("Can Traverse, Cooling Down");
if (cooldownTimer.IsRunning)
{
if ((cooldownTimer.ElapsedMilliseconds / 1000.0) > 1.0)
{
return(true);
}
else
{
return(false);
}
}
else
{
cooldownTimer.Reset();
cooldownTimer.Start();
return(false);
}
}
else
{
if (cooldownTimer.IsRunning)
{
cooldownTimer.Stop();
cooldownTimer.Reset();
}
return(false);
}
#endregion
}
/// <summary>
/// Set blockage time cost for the partition
/// </summary>
/// <param name="partition"></param>
/// <remarks>Should set interconnect blockage to be initial lane partition of next segment</remarks>
public void SetBlockageCost(IConnectAreaWaypoints partition)
{
// default time
partition.Blockage.BlockageTimeCost = 1800;
#region Old
/*
#region Lane Partition
// check if segment partition
if (partition is ArbiterLanePartition)
{
// get partition
ArbiterLanePartition alp = (ArbiterLanePartition)partition;
// make sure the other way is valid
bool otherValid = false;
// check 1
if (alp.Lane.Way.WayId.Number == 1)
{
if (alp.Lane.Way.Segment.Way2.IsValid)
otherValid = true;
}
// check 2
if (alp.Lane.Way.WayId.Number == 2)
{
if (alp.Lane.Way.Segment.Way1.IsValid)
otherValid = true;
}
// check other way valid
if (otherValid)
{
// determine adjacent partitions
List<NavigableEdge> toRemove = this.AdjacentPartitions(alp);
// get final waypoint of partition in other segment closest to partition's initial waypoint to plan from
ArbiterWay otherWay = null;
// check 1
if (alp.Lane.Way.WayId.Number == 1)
otherWay = alp.Lane.Way.Segment.Way2;
// check 2
if (alp.Lane.Way.WayId.Number == 2)
otherWay = alp.Lane.Way.Segment.Way1;
// get lanes of other way
ArbiterLane al = null;
foreach (ArbiterLane alTmp in otherWay.Lanes.Values)
{
al = alTmp;
}
// get closest waypoint
ArbiterWaypoint awOther = al.GetClosestPartition(alp.Initial.Position).Final;
// get replan point
List<DownstreamPointOfInterest> dpoi = this.Downstream(awOther.PreviousPartition.Initial.Position + awOther.PreviousPartition.Vector().Normalize(awOther.PreviousPartition.Length / 2.0), awOther.Lane.Way, true, new List<ArbiterWaypoint>());
ArbiterWaypoint nextOtherExit = dpoi != null && dpoi.Count > 0 ? dpoi[0].PointOfInterest : null;
// get next entry into lane
ArbiterWaypoint nextEntry = this.NextEntry(alp.Final);
if (nextEntry == null || nextOtherExit == null)
{
// default time
partition.Blockage.BlockageTimeCost = 10000;
}
else
{
// plan and set time
aStar planner = new aStar(nextOtherExit, nextEntry, toRemove);
planner.Plan();
partition.Blockage.BlockageTimeCost = planner.TotalTime;
}
}
else
{
// default time
partition.Blockage.BlockageTimeCost = 10000;
}
}
#endregion
*/
#endregion
}
/// <summary>
/// Remove blockage from a partition
/// </summary>
/// <param name="partition"></param>
public void RemoveBlockage(IConnectAreaWaypoints partition)
{
partition.Blockage.BlockageExists = false;
partition.Blockage.SecondsSinceObserved = 0.0;
if (partition is ArbiterLanePartition)
{
foreach (ArbiterLanePartition alp in ((ArbiterLanePartition)partition).NonLaneAdjacentPartitions)
{
if (alp.Blockage.BlockageExists && alp.Blockage.BlockageCoordinates.DistanceTo(partition.Blockage.BlockageCoordinates) < 1.0)
{
alp.Blockage.BlockageExists = false;
alp.Blockage.SecondsSinceObserved = 0.0;
}
}
}
}
/// <summary>
/// Checks if it is available for us to traverse the interconnect passed in
/// </summary>
/// <param name="ai"></param>
/// <returns></returns>
public bool CanTraverse(IConnectAreaWaypoints ai, VehicleState ourState)
{
// if we can go
bool? canGo = null;
// check for bug where interconnect not in intersection
if (!this.Intersection.PriorityLanes.ContainsKey(ai.ToInterconnect))
{
ArbiterOutput.Output("Exception: intersection: " + this.Intersection.ToString() + " priority lanes does not contain interconnect: " + ai.ToString() + " returning can go");
//ArbiterOutput.Output("Exception: intersection: " + this.Intersection.ToString() + " priority lanes does not contain interconnect: " + ai.ToString() + " returning can go");
//return true;
}
// check entry clear
if (!this.EntryAreaMonitor.Clear(ourState))
canGo = false;
// Check if we have priority if we're a stop
if (ai.InitialGeneric is ArbiterWaypoint && ((ArbiterWaypoint)ai.InitialGeneric).IsStop && IntersectionPriorityQueue[0].IsOurs)
{
// check if intersection clear at the moment
foreach (IntersectionVehicleMonitor ivm in this.InsideIntersectionVehicles)
{
if (!ivm.Failed(true))
canGo = false;
}
// check all priorities over us if clear
foreach (IIntersectionQueueable iiq in this.NonStopPriorityAreas)
{
if (!iiq.Clear(ourState))
canGo = false;
}
// clear
if(!canGo.HasValue)
canGo = true;
}
// check if not a stop if clear
else if (ai.InitialGeneric is ITraversableWaypoint && !((ITraversableWaypoint)ai.InitialGeneric).IsStop)
{
// get our monitor
IDominantMonitor ours = (IDominantMonitor)this.OurMonitor;
// check if intersection clear at the moment
foreach (IntersectionVehicleMonitor ivm in this.InsideIntersectionVehicles)
{
if (!ivm.Failed(true))
canGo = false;
}
// check all priorities over us if clear
foreach (IDominantMonitor iiq in this.NonStopPriorityAreas)
{
// checks if clear by default
bool isClear = iiq.Clear(ourState);
if (!isClear && !iiq.Waiting)
canGo = false;
if (iiq.Waiting)
{
ArbiterOutput.Output("DLEM: " + iiq.Area.ToString() + " vehicle waiting, hence clear");
}
}
// quick check
if (canGo.HasValue && this.PreviouslyWaiting.Count > 0)
canGo = false;
else
{
// check if previously waiting doesn't exist
foreach (IDominantMonitor idm in this.PreviouslyWaiting)
{
// check if waiting for this person excessively
if (!idm.ExcessiveWaiting)
canGo = false;
}
}
// clear
if (!canGo.HasValue)
canGo = true;
}
// nope, can't go if fall through
if (!canGo.HasValue)
canGo = false;
#region Cooldown
// final boolean if we can go
bool finalGo = canGo.Value;
if (finalGo)
{
ArbiterOutput.Output("Can Traverse, Cooling Down");
if(cooldownTimer.IsRunning)
{
if((cooldownTimer.ElapsedMilliseconds / 1000.0) > 1.0)
return true;
else
return false;
}
else
{
cooldownTimer.Reset();
cooldownTimer.Start();
return false;
}
}
else
{
if(cooldownTimer.IsRunning)
{
cooldownTimer.Stop();
cooldownTimer.Reset();
}
return false;
}
#endregion
}
/// <summary>
/// Checks if everything is clear (stops and priorities over the interconnect) so
/// that we don't have to stop
/// </summary>
/// <param name="ai"></param>
/// <returns></returns>
public bool IsAllClear(IConnectAreaWaypoints ai, VehicleState ourState)
{
// check if intersection clear at the moment
if (this.InsideIntersectionVehicles.Count > 0)
return false;
// check all monitors
foreach (IIntersectionQueueable iiq in this.AllMonitors)
{
if (!iiq.IsCompletelyClear(ourState))
return false;
}
// clear
return true;
}
public double DistanceTo(IConnectAreaWaypoints icaw)
{
return(icaw.DistanceTo(this.Zone.Perimeter.PerimeterPolygon.Center));
}
/// <summary>
/// Plans what maneuer we should take next
/// </summary>
/// <param name="planningState"></param>
/// <param name="navigationalPlan"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicles"></param>
/// <param name="obstacles"></param>
/// <param name="blockage"></param>
/// <returns></returns>
public Maneuver Plan(IState planningState, INavigationalPlan navigationalPlan, VehicleState vehicleState,
SceneEstimatorTrackedClusterCollection vehicles, SceneEstimatorUntrackedClusterCollection obstacles, List <ITacticalBlockage> blockages)
{
#region Waiting At Intersection Exit
if (planningState is WaitingAtIntersectionExitState)
{
// state
WaitingAtIntersectionExitState waies = (WaitingAtIntersectionExitState)planningState;
// get intersection plan
IntersectionPlan ip = (IntersectionPlan)navigationalPlan;
// nullify turn reasoning
this.TurnReasoning = null;
#region Intersection Monitor Updates
// check correct intersection monitor
if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(waies.exitWaypoint.AreaSubtypeWaypointId) &&
(IntersectionTactical.IntersectionMonitor == null ||
!IntersectionTactical.IntersectionMonitor.OurMonitor.Waypoint.Equals(waies.exitWaypoint)))
{
// create new intersection monitor
IntersectionTactical.IntersectionMonitor = new IntersectionMonitor(
waies.exitWaypoint,
CoreCommon.RoadNetwork.IntersectionLookup[waies.exitWaypoint.AreaSubtypeWaypointId],
vehicleState,
ip.BestOption);
}
// update if exists
if (IntersectionTactical.IntersectionMonitor != null)
{
// update monitor
IntersectionTactical.IntersectionMonitor.Update(vehicleState);
// print current
ArbiterOutput.Output(IntersectionTactical.IntersectionMonitor.IntersectionStateString());
}
#endregion
#region Desired Behavior
// get best option from previously saved
IConnectAreaWaypoints icaw = null;
if (waies.desired != null)
{
ArbiterInterconnect tmpInterconnect = waies.desired;
if (waies.desired.InitialGeneric is ArbiterWaypoint)
{
ArbiterWaypoint init = (ArbiterWaypoint)waies.desired.InitialGeneric;
if (init.NextPartition != null && init.NextPartition.Final.Equals(tmpInterconnect.FinalGeneric))
{
icaw = init.NextPartition;
}
else
{
icaw = waies.desired;
}
}
else
{
icaw = waies.desired;
}
}
else
{
icaw = ip.BestOption;
waies.desired = icaw.ToInterconnect;
}
#endregion
#region Turn Feasibility Reasoning
// check uturn
if (waies.desired.TurnDirection == ArbiterTurnDirection.UTurn)
{
waies.turnTestState = TurnTestState.Completed;
}
// check already determined feasible
if (waies.turnTestState == TurnTestState.Unknown ||
waies.turnTestState == TurnTestState.Failed)
{
#region Determine Behavior to Accomplish Turn
// get default turn behavior
TurnBehavior testTurnBehavior = this.DefaultTurnBehavior(icaw);
// set saudi decorator
if (waies.saudi != SAUDILevel.None)
{
testTurnBehavior.Decorators.Add(new ShutUpAndDoItDecorator(waies.saudi));
}
// set to ignore all vehicles
testTurnBehavior.VehiclesToIgnore = new List <int>(new int[] { -1 });
#endregion
#region Check Turn Feasible
// check if we have completed
CompletionReport turnCompletionReport;
bool completedTest = CoreCommon.Communications.TestExecute(testTurnBehavior, out turnCompletionReport); //CoreCommon.Communications.AsynchronousTestHasCompleted(testTurnBehavior, out turnCompletionReport, true);
// if we have completed the test
if (completedTest || ((TrajectoryBlockedReport)turnCompletionReport).BlockageType != BlockageType.Dynamic)
{
#region Can Complete
// check success
if (turnCompletionReport.Result == CompletionResult.Success)
{
// set completion state of the turn
waies.turnTestState = TurnTestState.Completed;
}
#endregion
#region No Saudi Level, Found Initial Blockage
// otherwise we cannot do the turn, check if saudi is still none
else if (waies.saudi == SAUDILevel.None)
{
// notify
ArbiterOutput.Output("Increased Saudi Level of Turn to L1");
// up the saudi level, set as turn failed and no other option
waies.saudi = SAUDILevel.L1;
waies.turnTestState = TurnTestState.Failed;
}
#endregion
#region Already at L1 Saudi
else if (waies.saudi == SAUDILevel.L1)
{
// notify
ArbiterOutput.Output("Turn with Saudi L1 Level failed");
// get an intersection plan without this interconnect
IntersectionPlan testPlan = CoreCommon.Navigation.PlanIntersectionWithoutInterconnect(
waies.exitWaypoint,
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId],
waies.desired);
// check that the plan exists
if (!testPlan.BestOption.ToInterconnect.Equals(waies.desired) &&
testPlan.BestRouteTime < double.MaxValue - 1.0)
{
// get the desired interconnect
ArbiterInterconnect reset = testPlan.BestOption.ToInterconnect;
#region Check that the reset interconnect is feasible
// test the reset interconnect
TurnBehavior testResetTurnBehavior = this.DefaultTurnBehavior(reset);
// set to ignore all vehicles
testResetTurnBehavior.VehiclesToIgnore = new List <int>(new int[] { -1 });
// check if we have completed
CompletionReport turnResetCompletionReport;
bool completedResetTest = CoreCommon.Communications.TestExecute(testResetTurnBehavior, out turnResetCompletionReport);
// check to see if this is feasible
if (completedResetTest && turnResetCompletionReport is SuccessCompletionReport && reset.Blockage.ProbabilityExists < 0.95)
{
// notify
ArbiterOutput.Output("Found clear interconnect: " + reset.ToString() + " adding blockage to current interconnect: " + waies.desired.ToString());
// set the interconnect as being blocked
CoreCommon.Navigation.AddInterconnectBlockage(waies.desired);
// reset all
waies.desired = reset;
waies.turnTestState = TurnTestState.Completed;
waies.saudi = SAUDILevel.None;
waies.useTurnBounds = true;
IntersectionMonitor.ResetDesired(reset);
}
#endregion
#region No Lane Bounds
// otherwise try without lane bounds
else
{
// notify
ArbiterOutput.Output("Had to fallout to using no turn bounds");
// up the saudi level, set as turn failed and no other option
waies.saudi = SAUDILevel.L1;
waies.turnTestState = TurnTestState.Completed;
waies.useTurnBounds = false;
}
#endregion
}
#region No Lane Bounds
// otherwise try without lane bounds
else
{
// up the saudi level, set as turn failed and no other option
waies.saudi = SAUDILevel.L1;
waies.turnTestState = TurnTestState.Unknown;
waies.useTurnBounds = false;
}
#endregion
}
#endregion
// want to reset ourselves
return(new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
#endregion
}
#endregion
#region Entry Monitor Blocked
// checks the entry monitor vehicle for failure
if (IntersectionMonitor != null && IntersectionMonitor.EntryAreaMonitor != null &&
IntersectionMonitor.EntryAreaMonitor.Vehicle != null && IntersectionMonitor.EntryAreaMonitor.Failed)
{
ArbiterOutput.Output("Entry area blocked");
// get an intersection plan without this interconnect
IntersectionPlan testPlan = CoreCommon.Navigation.PlanIntersectionWithoutInterconnect(
waies.exitWaypoint,
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId],
waies.desired,
true);
// check that the plan exists
if (!testPlan.BestOption.ToInterconnect.Equals(waies.desired) &&
testPlan.BestRouteTime < double.MaxValue - 1.0)
{
// get the desired interconnect
ArbiterInterconnect reset = testPlan.BestOption.ToInterconnect;
#region Check that the reset interconnect is feasible
// test the reset interconnect
TurnBehavior testResetTurnBehavior = this.DefaultTurnBehavior(reset);
// set to ignore all vehicles
testResetTurnBehavior.VehiclesToIgnore = new List <int>(new int[] { -1 });
// check if we have completed
CompletionReport turnResetCompletionReport;
bool completedResetTest = CoreCommon.Communications.TestExecute(testResetTurnBehavior, out turnResetCompletionReport);
// check to see if this is feasible
if (reset.TurnDirection == ArbiterTurnDirection.UTurn || (completedResetTest && turnResetCompletionReport is SuccessCompletionReport && reset.Blockage.ProbabilityExists < 0.95))
{
// notify
ArbiterOutput.Output("Found clear interconnect: " + reset.ToString() + " adding blockage to all possible turns into final");
// set all the interconnects to the final as being blocked
if (((ITraversableWaypoint)waies.desired.FinalGeneric).IsEntry)
{
foreach (ArbiterInterconnect toBlock in ((ITraversableWaypoint)waies.desired.FinalGeneric).Entries)
{
CoreCommon.Navigation.AddInterconnectBlockage(toBlock);
}
}
// check if exists previous partition to block
if (waies.desired.FinalGeneric is ArbiterWaypoint)
{
ArbiterWaypoint finWaypoint = (ArbiterWaypoint)waies.desired.FinalGeneric;
if (finWaypoint.PreviousPartition != null)
{
CoreCommon.Navigation.AddBlockage(finWaypoint.PreviousPartition, finWaypoint.Position, false);
}
}
// reset all
waies.desired = reset;
waies.turnTestState = TurnTestState.Completed;
waies.saudi = SAUDILevel.None;
waies.useTurnBounds = true;
IntersectionMonitor.ResetDesired(reset);
// want to reset ourselves
return(new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
#endregion
}
else
{
ArbiterOutput.Output("Entry area blocked, but no otehr valid route found");
}
}
#endregion
// check if can traverse
if (IntersectionTactical.IntersectionMonitor == null || IntersectionTactical.IntersectionMonitor.CanTraverse(icaw, vehicleState))
{
#region If can traverse the intersection
// quick check not interconnect
if (!(icaw is ArbiterInterconnect))
{
icaw = icaw.ToInterconnect;
}
// get interconnect
ArbiterInterconnect ai = (ArbiterInterconnect)icaw;
// clear all old completion reports
CoreCommon.Communications.ClearCompletionReports();
// check if uturn
if (ai.InitialGeneric is ArbiterWaypoint && ai.FinalGeneric is ArbiterWaypoint && ai.TurnDirection == ArbiterTurnDirection.UTurn)
{
// go into turn
List <ArbiterLane> involvedLanes = new List <ArbiterLane>();
involvedLanes.Add(((ArbiterWaypoint)ai.InitialGeneric).Lane);
involvedLanes.Add(((ArbiterWaypoint)ai.FinalGeneric).Lane);
uTurnState nextState = new uTurnState(((ArbiterWaypoint)ai.FinalGeneric).Lane,
IntersectionToolkit.uTurnBounds(vehicleState, involvedLanes));
nextState.Interconnect = ai;
// hold brake
Behavior b = new HoldBrakeBehavior();
// return maneuver
return(new Maneuver(b, nextState, nextState.DefaultStateDecorators, vehicleState.Timestamp));
}
else
{
if (ai.FinalGeneric is ArbiterWaypoint)
{
ArbiterWaypoint finalWaypoint = (ArbiterWaypoint)ai.FinalGeneric;
// get turn params
LinePath finalPath;
LineList leftLL;
LineList rightLL;
IntersectionToolkit.TurnInfo(finalWaypoint, out finalPath, out leftLL, out rightLL);
// go into turn
IState nextState = new TurnState(ai, ai.TurnDirection, finalWaypoint.Lane, finalPath, leftLL, rightLL, new ScalarSpeedCommand(2.5), waies.saudi, waies.useTurnBounds);
// hold brake
Behavior b = new HoldBrakeBehavior();
// return maneuver
return(new Maneuver(b, nextState, nextState.DefaultStateDecorators, vehicleState.Timestamp));
}
else
{
// final perimeter waypoint
ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)ai.FinalGeneric;
// get turn params
LinePath finalPath;
LineList leftLL;
LineList rightLL;
IntersectionToolkit.ZoneTurnInfo(ai, apw, out finalPath, out leftLL, out rightLL);
// go into turn
IState nextState = new TurnState(ai, ai.TurnDirection, null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(2.5), waies.saudi, waies.useTurnBounds);
// hold brake
Behavior b = new HoldBrakeBehavior();
// return maneuver
return(new Maneuver(b, nextState, nextState.DefaultStateDecorators, vehicleState.Timestamp));
}
}
#endregion
}
// otherwise need to wait
else
{
IState next = waies;
return(new Maneuver(new HoldBrakeBehavior(), next, next.DefaultStateDecorators, vehicleState.Timestamp));
}
}
#endregion
#region Stopping At Exit
else if (planningState is StoppingAtExitState)
{
// cast to exit stopping
StoppingAtExitState saes = (StoppingAtExitState)planningState;
saes.currentPosition = vehicleState.Front;
// get intersection plan
IntersectionPlan ip = (IntersectionPlan)navigationalPlan;
// if has an intersection
if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(saes.waypoint.AreaSubtypeWaypointId))
{
// create new intersection monitor
IntersectionTactical.IntersectionMonitor = new IntersectionMonitor(
saes.waypoint,
CoreCommon.RoadNetwork.IntersectionLookup[saes.waypoint.AreaSubtypeWaypointId],
vehicleState,
ip.BestOption);
// update it
IntersectionTactical.IntersectionMonitor.Update(vehicleState);
}
else
{
IntersectionTactical.IntersectionMonitor = null;
}
// otherwise update the stop parameters
saes.currentPosition = vehicleState.Front;
Behavior b = saes.Resume(vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value);
return(new Maneuver(b, saes, saes.DefaultStateDecorators, vehicleState.Timestamp));
}
#endregion
#region In uTurn
else if (planningState is uTurnState)
{
// get state
uTurnState uts = (uTurnState)planningState;
// check if in other lane
if (CoreCommon.Communications.HasCompleted((new UTurnBehavior(null, null, null, null)).GetType()))
{
// quick check
if (uts.Interconnect != null && uts.Interconnect.FinalGeneric is ArbiterWaypoint)
{
// get the closest partition to the new lane
ArbiterLanePartition alpClose = uts.TargetLane.GetClosestPartition(vehicleState.Front);
// waypoints
ArbiterWaypoint partitionInitial = alpClose.Initial;
ArbiterWaypoint uturnEnd = (ArbiterWaypoint)uts.Interconnect.FinalGeneric;
// check initial past end
if (partitionInitial.WaypointId.Number > uturnEnd.WaypointId.Number)
{
// get waypoints inclusive
List <ArbiterWaypoint> inclusive = uts.TargetLane.WaypointsInclusive(uturnEnd, partitionInitial);
bool found = false;
// loop through
foreach (ArbiterWaypoint aw in inclusive)
{
if (!found && aw.WaypointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId))
{
// notiofy
ArbiterOutput.Output("removed checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString() + " as passed over in uturn");
// remove
CoreCommon.Mission.MissionCheckpoints.Dequeue();
// set found
found = true;
}
}
}
// default check
else if (uts.Interconnect.FinalGeneric.Equals(CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId]))
{
// notiofy
ArbiterOutput.Output("removed checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString() + " as end of uturn");
// remove
CoreCommon.Mission.MissionCheckpoints.Dequeue();
}
}
// check if the uturn is for a blockage
else if (uts.Interconnect == null)
{
// get final lane
ArbiterLane targetLane = uts.TargetLane;
// check has opposing
if (targetLane.Way.Segment.Lanes.Count > 1)
{
// check the final checkpoint is in our lane
if (CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.AreaSubtypeId.Equals(targetLane.LaneId))
{
// check that the final checkpoint is within the uturn polygon
if (uts.Polygon != null &&
uts.Polygon.IsInside(CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId].Position))
{
// remove the checkpoint
ArbiterOutput.Output("Found checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.ToString() + " inside blockage uturn area, dequeuing");
CoreCommon.Mission.MissionCheckpoints.Dequeue();
}
}
}
}
// stay in target lane
IState nextState = new StayInLaneState(uts.TargetLane, new Probability(0.8, 0.2), true, CoreCommon.CorePlanningState);
Behavior b = new StayInLaneBehavior(uts.TargetLane.LaneId, new ScalarSpeedCommand(2.0), new List <int>(), uts.TargetLane.LanePath(), uts.TargetLane.Width, uts.TargetLane.NumberOfLanesLeft(vehicleState.Front, true), uts.TargetLane.NumberOfLanesRight(vehicleState.Front, true));
return(new Maneuver(b, nextState, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
// otherwise continue uturn
else
{
// get polygon
Polygon p = uts.Polygon;
// add polygon to observable
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(p, ArbiterInformationDisplayObjectType.uTurnPolygon));
// check the type of uturn
if (!uts.singleLaneUturn)
{
// get ending path
LinePath endingPath = uts.TargetLane.LanePath();
// next state is current
IState nextState = uts;
// behavior
Behavior b = new UTurnBehavior(p, endingPath, uts.TargetLane.LaneId, new ScalarSpeedCommand(2.0));
// maneuver
return(new Maneuver(b, nextState, null, vehicleState.Timestamp));
}
else
{
// get ending path
LinePath endingPath = uts.TargetLane.LanePath().Clone();
endingPath = endingPath.ShiftLateral(-2.0); //uts.TargetLane.Width);
// add polygon to observable
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(endingPath, ArbiterInformationDisplayObjectType.leftBound));
// next state is current
IState nextState = uts;
// behavior
Behavior b = new UTurnBehavior(p, endingPath, uts.TargetLane.LaneId, new ScalarSpeedCommand(2.0));
// maneuver
return(new Maneuver(b, nextState, null, vehicleState.Timestamp));
}
}
}
#endregion
#region In Turn
else if (planningState is TurnState)
{
// get state
TurnState ts = (TurnState)planningState;
// add bounds to observable
if (ts.LeftBound != null && ts.RightBound != null)
{
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(ts.LeftBound, ArbiterInformationDisplayObjectType.leftBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(ts.RightBound, ArbiterInformationDisplayObjectType.rightBound));
}
// create current turn reasoning
if (this.TurnReasoning == null)
{
this.TurnReasoning = new TurnReasoning(ts.Interconnect,
IntersectionTactical.IntersectionMonitor != null ? IntersectionTactical.IntersectionMonitor.EntryAreaMonitor : null);
}
// get primary maneuver
Maneuver primary = this.TurnReasoning.PrimaryManeuver(vehicleState, blockages, ts);
// get secondary maneuver
Maneuver?secondary = this.TurnReasoning.SecondaryManeuver(vehicleState, (IntersectionPlan)navigationalPlan);
// return the manevuer
return(secondary.HasValue ? secondary.Value : primary);
}
#endregion
#region Itnersection Startup
else if (planningState is IntersectionStartupState)
{
// state and plan
IntersectionStartupState iss = (IntersectionStartupState)planningState;
IntersectionStartupPlan isp = (IntersectionStartupPlan)navigationalPlan;
// initial path
LinePath vehiclePath = new LinePath(new Coordinates[] { vehicleState.Rear, vehicleState.Front });
List <ITraversableWaypoint> feasibleEntries = new List <ITraversableWaypoint>();
// vehicle polygon forward of us
Polygon vehicleForward = vehicleState.ForwardPolygon;
// best waypoint
ITraversableWaypoint best = null;
double bestCost = Double.MaxValue;
// given feasible choose best, no feasible choose random
if (feasibleEntries.Count == 0)
{
foreach (ITraversableWaypoint itw in iss.Intersection.AllEntries.Values)
{
if (vehicleForward.IsInside(itw.Position))
{
feasibleEntries.Add(itw);
}
}
if (feasibleEntries.Count == 0)
{
foreach (ITraversableWaypoint itw in iss.Intersection.AllEntries.Values)
{
feasibleEntries.Add(itw);
}
}
}
// get best
foreach (ITraversableWaypoint itw in feasibleEntries)
{
if (isp.NodeTimeCosts.ContainsKey(itw))
{
KeyValuePair <ITraversableWaypoint, double> lookup = new KeyValuePair <ITraversableWaypoint, double>(itw, isp.NodeTimeCosts[itw]);
if (best == null || lookup.Value < bestCost)
{
best = lookup.Key;
bestCost = lookup.Value;
}
}
}
// get something going to this waypoint
ArbiterInterconnect interconnect = null;
if (best.IsEntry)
{
ArbiterInterconnect closest = null;
double closestDistance = double.MaxValue;
foreach (ArbiterInterconnect ai in best.Entries)
{
double dist = ai.InterconnectPath.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front);
if (closest == null || dist < closestDistance)
{
closest = ai;
closestDistance = dist;
}
}
interconnect = closest;
}
else if (best is ArbiterWaypoint && ((ArbiterWaypoint)best).PreviousPartition != null)
{
interconnect = ((ArbiterWaypoint)best).PreviousPartition.ToInterconnect;
}
// get state
if (best is ArbiterWaypoint)
{
// go to this turn state
LinePath finalPath;
LineList leftBound;
LineList rightBound;
IntersectionToolkit.TurnInfo((ArbiterWaypoint)best, out finalPath, out leftBound, out rightBound);
return(new Maneuver(new HoldBrakeBehavior(), new TurnState(interconnect, interconnect.TurnDirection, ((ArbiterWaypoint)best).Lane,
finalPath, leftBound, rightBound, new ScalarSpeedCommand(2.0)), TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
else
{
// go to this turn state
LinePath finalPath;
LineList leftBound;
LineList rightBound;
IntersectionToolkit.ZoneTurnInfo(interconnect, (ArbiterPerimeterWaypoint)best, out finalPath, out leftBound, out rightBound);
return(new Maneuver(new HoldBrakeBehavior(), new TurnState(interconnect, interconnect.TurnDirection, null,
finalPath, leftBound, rightBound, new ScalarSpeedCommand(2.0)), TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
}
#endregion
#region Unknown
else
{
throw new Exception("Unknown planning state in intersection tactical plan: " + planningState.ToString());
}
#endregion
}
