public WaitingAtIntersectionExitState(ITraversableWaypoint exit, ArbiterTurnDirection turnDirection, IntersectionDescription description, ArbiterInterconnect desired)
{
this.exitWaypoint = exit;
this.turnDirection = turnDirection;
this.IntersectionDescription = description;
this.desired = desired;
this.turnTestState = TurnTestState.Unknown;
}
public WaitingAtIntersectionExitState(ITraversableWaypoint exit, ArbiterTurnDirection turnDirection, IntersectionDescription description, ArbiterInterconnect desired)
{
this.exitWaypoint = exit;
this.turnDirection = turnDirection;
this.IntersectionDescription = description;
this.desired = desired;
this.turnTestState = TurnTestState.Unknown;
}
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="lane"></param>
/// <param name="waypoint"></param>
/// <param name="turnDirection"></param>
/// <param name="isNavigationExit"></param>
/// <param name="desiredExit"></param>
public StoppingAtStopState(ArbiterLane lane, ArbiterWaypoint waypoint, ArbiterTurnDirection turnDirection,
bool isNavigationExit, ArbiterInterconnect desiredExit)
{
this.lane = lane;
this.waypoint = waypoint;
this.turnDirection = turnDirection;
this.isNavigationExit = isNavigationExit;
this.desiredExit = desiredExit;
this.internalLaneState = new InternalState(lane.LaneId, lane.LaneId);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="turnFinal"></param>
public ZoneAreaEntryMonitor(ArbiterPerimeterWaypoint turnFinal, ArbiterInterconnect ai, bool isOurs,
IntersectionMonitor globalMonitor, IntersectionInvolved involved)
{
this.finalWaypoint = turnFinal;
this.entryPolygon = this.GenerateEntryMonitorPolygon(ai);
this.failedTimer = new Stopwatch();
this.isOurs = isOurs;
this.globalMonitor = globalMonitor;
this.involved = involved;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="lane"></param>
/// <param name="waypoint"></param>
/// <param name="turnDirection"></param>
/// <param name="isNavigationExit"></param>
/// <param name="desiredExit"></param>
public StoppingAtStopState(ArbiterLane lane, ArbiterWaypoint waypoint, ArbiterTurnDirection turnDirection,
bool isNavigationExit, ArbiterInterconnect desiredExit)
{
this.lane = lane;
this.waypoint = waypoint;
this.turnDirection = turnDirection;
this.isNavigationExit = isNavigationExit;
this.desiredExit = desiredExit;
this.internalLaneState = new InternalState(lane.LaneId, lane.LaneId);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="turnFinal"></param>
public DominantZoneEntryMonitor(ArbiterPerimeterWaypoint turnFinal, ArbiterInterconnect ai, bool isOurs,
IntersectionMonitor globalMonitor, IntersectionInvolved involved)
{
this.finalWaypoint = turnFinal;
this.entryPolygon = this.GenerateEntryMonitorPolygon(ai);
this.failedTimer = new Stopwatch();
this.isOurs = isOurs;
this.globalMonitor = globalMonitor;
this.involved = involved;
}
/// <summary>
/// Turn information
/// </summary>
/// <param name="entry"></param>
/// <param name="finalPath"></param>
/// <param name="leftBound"></param>
/// <param name="rightBound"></param>
public static void ZoneTurnInfo(ArbiterInterconnect ai, ArbiterPerimeterWaypoint entry, out LinePath finalPath, out LineList leftBound, out LineList rightBound)
{
//Coordinates centerVec = entry.Perimeter.PerimeterPolygon.CalculateBoundingCircle().center - entry.Position;
Coordinates centerVec = ai.InterconnectPath[1] - ai.InterconnectPath[0];
centerVec = centerVec.Normalize(TahoeParams.VL);
finalPath = new LinePath(new Coordinates[] { entry.Position, entry.Position + centerVec });
leftBound = finalPath.ShiftLateral(TahoeParams.T * 2.0);
rightBound = finalPath.ShiftLateral(-TahoeParams.T * 2.0);
}
/// <summary>
/// Route ploan for downstream exits
/// </summary>
/// <param name="downstreamPoints"></param>
/// <param name="goal"></param>
private void DetermineDownstreamPointRouteTimes(List <DownstreamPointOfInterest> downstreamPoints, INavigableNode goal, ArbiterWay aw)
{
// so, for each exit downstream we need to plan from the end of each interconnect to the goal
foreach (DownstreamPointOfInterest dpoi in downstreamPoints)
{
// check if exit
if (dpoi.IsExit)
{
// current best time
double bestCurrent = Double.MaxValue;
List <INavigableNode> bestRoute = null;
ArbiterInterconnect bestInterconnect = null;
// fake node
FakeExitNode fen = new FakeExitNode(dpoi.PointOfInterest);
// init fields
double timeCost;
List <INavigableNode> routeNodes;
// plan
this.Plan(fen, goal, out routeNodes, out timeCost);
bestCurrent = timeCost;
bestRoute = routeNodes;
bestInterconnect = routeNodes.Count > 1 ? fen.GetEdge(routeNodes[1]) : null;
// plan from each interconnect to find the best time from exit
/*foreach (ArbiterInterconnect ai in dpoi.PointOfInterest.Exits)
* {
* // init fields
* double timeCost;
* List<INavigableNode> routeNodes;
*
* // plan
* this.Plan(ai.End, goal, out routeNodes, out timeCost);
*
* // check
* if (timeCost < bestCurrent)
* {
* bestCurrent = timeCost;
* bestRoute = routeNodes;
* bestInterconnect = ai;
* }
* }*/
// set best
dpoi.RouteTime = bestCurrent;
dpoi.BestRoute = bestRoute;
dpoi.BestExit = bestInterconnect;
}
}
}
/// <summary>
/// Makes polygon representing the entry
/// </summary>
public Polygon GenerateEntryMonitorPolygon(ArbiterInterconnect ai)
{
Coordinates aiVector = ai.FinalGeneric.Position - ai.InitialGeneric.Position;
Coordinates aiEntry = this.finalWaypoint.Position + aiVector.Normalize(TahoeParams.VL * 1.5);
Coordinates centerVector = this.finalWaypoint.Perimeter.PerimeterPolygon.CalculateBoundingCircle().center - this.finalWaypoint.Position;
Coordinates centerEntry = this.finalWaypoint.Position + centerVector.Normalize(TahoeParams.VL * 1.5);
List <Coordinates> boundingCoords = new List <Coordinates>(new Coordinates[] { aiEntry, centerEntry, finalWaypoint.Position });
return(Polygon.GrahamScan(boundingCoords).Inflate(2.0 * TahoeParams.T));
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="interconnect"></param>
/// <param name="turn"></param>
public TurnState(ArbiterInterconnect interconnect, ArbiterTurnDirection turn, ArbiterLane target, LinePath endingPath, LineList left,
LineList right, SpeedCommand speed, SAUDILevel saudi, bool useTurnBounds)
{
this.Interconnect = interconnect;
this.turnDirection = turn;
this.TargetLane = target;
this.EndingPath = endingPath;
this.LeftBound = left;
this.RightBound = right;
this.SpeedCommand = speed;
this.Saudi = saudi;
this.UseTurnBounds = useTurnBounds;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="interconnect"></param>
/// <param name="turn"></param>
public TurnState(ArbiterInterconnect interconnect, ArbiterTurnDirection turn, ArbiterLane target, LinePath endingPath, LineList left,
LineList right, SpeedCommand speed, SAUDILevel saudi, bool useTurnBounds)
{
this.Interconnect = interconnect;
this.turnDirection = turn;
this.TargetLane = target;
this.EndingPath = endingPath;
this.LeftBound = left;
this.RightBound = right;
this.SpeedCommand = speed;
this.Saudi = saudi;
this.UseTurnBounds = useTurnBounds;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="lane"></param>
/// <param name="waypoint"></param>
/// <param name="turnDirection"></param>
/// <param name="isNavigationExit"></param>
/// <param name="desiredExit"></param>
public StoppingAtExitState(ArbiterLane lane, ArbiterWaypoint waypoint, ArbiterTurnDirection turnDirection,
bool isNavigationExit, ArbiterInterconnect desiredExit, double timeStamp, Coordinates currentPosition)
{
this.lane = lane;
this.waypoint = waypoint;
this.turnDirection = turnDirection;
this.isNavigationExit = isNavigationExit;
this.desiredExit = desiredExit;
this.internalLaneState = new InternalState(lane.LaneId, lane.LaneId);
this.timeStamp = timeStamp;
this.currentPosition = currentPosition;
this.internalLaneState = new InternalState(this.lane.LaneId, this.lane.LaneId);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="lane"></param>
/// <param name="waypoint"></param>
/// <param name="turnDirection"></param>
/// <param name="isNavigationExit"></param>
/// <param name="desiredExit"></param>
public StoppingAtExitState(ArbiterLane lane, ArbiterWaypoint waypoint, ArbiterTurnDirection turnDirection,
bool isNavigationExit, ArbiterInterconnect desiredExit, double timeStamp, Coordinates currentPosition)
{
this.lane = lane;
this.waypoint = waypoint;
this.turnDirection = turnDirection;
this.isNavigationExit = isNavigationExit;
this.desiredExit = desiredExit;
this.internalLaneState = new InternalState(lane.LaneId, lane.LaneId);
this.timeStamp = timeStamp;
this.currentPosition = currentPosition;
this.internalLaneState = new InternalState(this.lane.LaneId, this.lane.LaneId);
}
private static Polygon GenerateMiddlePathPolygon(LinePath initial, LinePath middle, LinePath final, ArbiterLane lane)
{
// wp's
ArbiterWaypoint w1 = new ArbiterWaypoint(initial[0], new ArbiterWaypointId(1, lane.LaneId));
ArbiterWaypoint w2 = new ArbiterWaypoint(initial[1], new ArbiterWaypointId(2, lane.LaneId));
ArbiterWaypoint w3 = new ArbiterWaypoint(final[0], new ArbiterWaypointId(3, lane.LaneId));
ArbiterWaypoint w4 = new ArbiterWaypoint(final[1], new ArbiterWaypointId(4, lane.LaneId));
// set lane
w1.Lane = lane;
w2.Lane = lane;
w3.Lane = lane;
w4.Lane = lane;
// alps
ArbiterLanePartition alp1 = new ArbiterLanePartition(new ArbiterLanePartitionId(w1.WaypointId, w2.WaypointId, lane.LaneId), w1, w2, lane.Way.Segment);
ArbiterLanePartition alp2 = new ArbiterLanePartition(new ArbiterLanePartitionId(w2.WaypointId, w3.WaypointId, lane.LaneId), w2, w3, lane.Way.Segment);
ArbiterLanePartition alp3 = new ArbiterLanePartition(new ArbiterLanePartitionId(w3.WaypointId, w4.WaypointId, lane.LaneId), w3, w4, lane.Way.Segment);
// set links
w1.NextPartition = alp1;
w2.NextPartition = alp2;
w3.NextPartition = alp3;
w4.PreviousPartition = alp3;
w3.PreviousPartition = alp2;
w2.PreviousPartition = alp1;
// get poly
ArbiterTurnDirection atd = PartitionTurnDirection(alp2);
if (atd != ArbiterTurnDirection.Straight)
{
ArbiterInterconnect ai = alp2.ToInterconnect;
ai.TurnDirection = atd;
GenerateInterconnectPolygon(ai);
return(ai.TurnPolygon);
}
return(null);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initial"></param>
/// <param name="next"></param>
/// <param name="final"></param>
public SupraLane(ArbiterLane initial, ArbiterInterconnect next, ArbiterLane final)
{
Components = new SupraLaneComponentList();
Components.Add(initial);
Components.Add(next);
Components.Add(final);
this.Initial = initial;
this.Final = final;
this.Interconnect = next;
if (!this.Initial.Equals(this.Final))
{
this.supraWaypoints = new List<ArbiterWaypoint>();
this.supraWaypoints.AddRange(this.Initial.WaypointsInclusive(this.Initial.WaypointList[0], (ArbiterWaypoint)this.Interconnect.InitialGeneric));
this.supraWaypoints.AddRange(this.Final.WaypointsInclusive((ArbiterWaypoint)this.Interconnect.FinalGeneric, this.Final.WaypointList[this.Final.WaypointList.Count - 1]));
this.SetDefaultLanePath();
}
else
{
this.supraWaypoints = this.Initial.WaypointList;
this.supraPath = this.Initial.LanePath();
this.supraPath.Add(this.Interconnect.FinalGeneric.Position);
}
}
/// <summary>
/// Get lanes that overlap with hte interconnect
/// </summary>
/// <param name="lanes"></param>
/// <param name="ai"></param>
/// <returns></returns>
private List<IntersectionInvolved> laneOverlaps(List<ArbiterLane> lanes, ArbiterInterconnect ai, IEnumerable<ITraversableWaypoint> exits)
{
List<IntersectionInvolved> overlaps = new List<IntersectionInvolved>();
LineSegment aiSegment = new LineSegment(ai.InitialGeneric.Position, ai.FinalGeneric.Position);
if (ai.FinalGeneric is ArbiterWaypoint)
{
ArbiterWaypoint fin = (ArbiterWaypoint)ai.FinalGeneric;
if (fin.PreviousPartition != null && !this.FoundStop(fin.PreviousPartition.Initial, exits, fin.Lane))
{
ArbiterWaypoint foundExit = null;
foreach (ITraversableWaypoint itw in exits)
{
if (itw is ArbiterWaypoint && ((ArbiterWaypoint)itw).Lane.Equals(fin.Lane) &&
(foundExit == null || itw.Position.DistanceTo(fin.Position) < foundExit.Position.DistanceTo(fin.Position)))
foundExit = (ArbiterWaypoint)itw;
}
if (foundExit != null)
overlaps.Add(new IntersectionInvolved(foundExit, fin.Lane, ArbiterTurnDirection.Straight));
else
overlaps.Add(new IntersectionInvolved(fin.Lane));
}
}
foreach (ArbiterLane al in lanes)
{
if (!(ai.InitialGeneric is ArbiterWaypoint) || !((ArbiterWaypoint)ai.InitialGeneric).Lane.Equals(al))
{
foreach (LineSegment ls in al.LanePath().GetSegmentEnumerator())
{
Coordinates interCoord;
bool intersect = ls.Intersect(aiSegment, out interCoord);
/*if (intersect)
{
Console.WriteLine("");
}*/
if (intersect && ai.IsInside(interCoord) && !overlaps.Contains(new IntersectionInvolved(al)) &&
ai.InterconnectPath.GetClosestPoint(interCoord).Location.DistanceTo(interCoord) < 0.00001 && al.IsInside(interCoord))
{
// get closest partition
ArbiterLanePartition alp = al.GetClosestPartition(interCoord);
if (!this.FoundStop(alp.Initial, exits, al))
{
ArbiterWaypoint foundExit = null;
foreach (ITraversableWaypoint itw in exits)
{
if (itw is ArbiterWaypoint && ((ArbiterWaypoint)itw).Lane.Equals(alp.Lane) &&
(foundExit == null || itw.Position.DistanceTo(interCoord) < foundExit.Position.DistanceTo(interCoord)))
foundExit = (ArbiterWaypoint)itw;
}
if (foundExit != null)
overlaps.Add(new IntersectionInvolved(foundExit, alp.Lane, ArbiterTurnDirection.Straight));
else
overlaps.Add(new IntersectionInvolved(al));
}
}
}
}
}
return overlaps;
}
/// <summary>
/// Resets the interconnect we wish to take
/// </summary>
/// <param name="reset"></param>
public void ResetDesired(ArbiterInterconnect desired)
{
// create monitors
this.AllMonitors = this.IntersectionPriorityQueue;
this.NonStopPriorityAreas = new List<IDominantMonitor>();
this.PreviouslyWaiting = new List<IDominantMonitor>();
this.cooldownTimer = new Stopwatch();
#region Priority Areas Over Interconnect
// check contains priority lane for this
if (this.Intersection.PriorityLanes.ContainsKey(desired.ToInterconnect))
{
// loop through all other priority monitors over this interconnect
foreach (IntersectionInvolved ii in this.Intersection.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(this.OurMonitor.Waypoint, ((ArbiterPerimeterWaypoint)desired.FinalGeneric).Perimeter.Zone,
((ArbiterInterconnect)desired).TurnDirection));
}
else
{
throw new Exception("unhandled desired interconnect final type");
}
#endregion
// add to all
foreach (IIntersectionQueueable iiq in this.NonStopPriorityAreas)
this.AllMonitors.Add(iiq);
this.AllMonitors.Add(this.EntryAreaMonitor);
// update all
this.Update(CoreCommon.Communications.GetVehicleState());
}
/// <summary>
/// Gets parameters for following forward vehicle
/// </summary>
/// <param name="vehicleState"></param>
/// <param name="fullPath"></param>
/// <param name="followingSpeed"></param>
/// <param name="distanceToGood"></param>
public void Follow(VehicleState state, LinePath fullPath, ArbiterLane lane, ArbiterInterconnect turn,
out double followingSpeed, out double distanceToGood, out double xSep)
{
// get the maximum velocity of the segment we're closest to
double segV = Math.Min(lane.CurrentMaximumSpeed(state.Front), turn.MaximumDefaultSpeed);
double segMaxV = segV;
// minimum distance
double xAbsMin = TahoeParams.VL * 1.5;
// retrieve the tracked vehicle's scalar absolute speed
double vTarget = CurrentVehicle.StateMonitor.Observed.isStopped ? 0.0 : this.CurrentVehicle.Speed;
// get the good distance behind the target, is xAbsMin if vehicle velocity is small enough
double xGood = xAbsMin + (1.5 * (TahoeParams.VL / 4.4704) * vTarget);
// get our current separation
double xSeparation = state.Front.DistanceTo(this.CurrentVehicle.ClosestPosition); //lane.DistanceBetween(state.Front, this.CurrentVehicle.ClosestPosition);
xSep = xSeparation;
// determine the envelope to reason about the vehicle, that is, to slow from vMax to vehicle speed
double xEnvelope = (Math.Pow(vTarget, 2.0) - Math.Pow(segMaxV, 2.0)) / (2.0 * -0.5);
// the distance to the good
double xDistanceToGood;
// determine the velocity to follow the vehicle ahead
double vFollowing;
// check if we are basically stopped in the right place behind another vehicle
if (vTarget < 1 && Math.Abs(xSeparation - xGood) < 1)
{
// stop
vFollowing = 0;
// good distance
xDistanceToGood = 0;
}
// check if we are within the minimum distance
else if (xSeparation <= xAbsMin)
{
// stop
vFollowing = 0;
// good distance
xDistanceToGood = 0;
}
// determine if our separation is less than the good distance but not inside minimum
else if (xAbsMin < xSeparation && xSeparation < xGood)
{
// get the distance we are from xMin
double xAlong = xSeparation - xAbsMin;
// get the total distance from xGood to xMin
double xGoodToMin = xGood - xAbsMin;
// slow to 0 by min
vFollowing = (((xGoodToMin - xAlong) / xGoodToMin) * (-vTarget)) + vTarget;
// good distance
xDistanceToGood = 0;
}
// our separation is greater than the good distance but within the envelope
else if (xGood <= xSeparation && xSeparation <= xEnvelope + xGood)
{
// get differences in max and target velocities
double vDifference = Math.Max(segMaxV - vTarget, 0);
// get the distance we are along the speed envolope from xGood
double xAlong = xEnvelope - (xSeparation - xGood);
// slow to vTarget by good
vFollowing = (((xEnvelope - xAlong) / xEnvelope) * (vDifference)) + vTarget;
// good distance
xDistanceToGood = xSeparation - xGood;
}
// otherwise xSeparation > xEnvelope
else
{
// can go max speed
vFollowing = segMaxV;
// good distance
xDistanceToGood = xSeparation - xGood;
}
// set out params
followingSpeed = vFollowing;
distanceToGood = xDistanceToGood;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="interconnect"></param>
/// <param name="timeCost"></param>
/// <param name="nodes"></param>
public PlanableInterconnect(ArbiterInterconnect interconnect, double timeCost, List<INavigableNode> nodes)
{
this.Interconnect = interconnect;
this.TimeCost = timeCost;
this.PlannedNodes = nodes;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="turn"></param>
public TurnReasoning(ArbiterInterconnect turn, IEntryAreaMonitor entryAreaMonitor)
{
this.turn = turn;
this.navigation = new Navigator();
this.forwardMonitor = new TurnForwardMonitor(turn, entryAreaMonitor);
}
/// <summary>
/// Check where turns intersect the subpath furthese along it
/// </summary>
/// <param name="aiSubpath"></param>
/// <param name="iTraversableWaypoint"></param>
/// <returns></returns>
private Coordinates?TurnIntersects(LinePath aiSubpath, ITraversableWaypoint iTraversableWaypoint, out ArbiterInterconnect interconnect)
{
Line aiSub = new Line(aiSubpath.StartPoint.Location, aiSubpath.EndPoint.Location);
double distance = 0.0;
Coordinates?furthest = null;
interconnect = null;
foreach (ArbiterInterconnect ai in iTraversableWaypoint.OutgoingConnections)
{
Line iLine = new Line(ai.InterconnectPath.StartPoint.Location, ai.InterconnectPath.EndPoint.Location);
Coordinates intersect;
bool doesIntersect = aiSub.Intersect(iLine, out intersect);
if (doesIntersect && (ai.IsInside(intersect) || ai.InterconnectPath.GetClosestPoint(intersect).Equals(ai.InterconnectPath.EndPoint)))
{
if (!furthest.HasValue)
{
furthest = intersect;
distance = intersect.DistanceTo(aiSubpath.EndPoint.Location);
interconnect = ai;
}
else
{
double tmpDist = intersect.DistanceTo(aiSubpath.EndPoint.Location);
if (tmpDist < distance)
{
furthest = intersect;
distance = tmpDist;
interconnect = ai;
}
}
}
}
if (furthest.HasValue)
{
return(furthest.Value);
}
else
{
return(null);
}
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="interconnect"></param>
/// <param name="timeCost"></param>
/// <param name="nodes"></param>
public PlanableInterconnect(ArbiterInterconnect interconnect, double timeCost, List <INavigableNode> nodes)
{
this.Interconnect = interconnect;
this.TimeCost = timeCost;
this.PlannedNodes = nodes;
}
/// <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
}
public void GenerateInterconnectPolygon(ArbiterInterconnect ai)
{
List<Coordinates> polyPoints = new List<Coordinates>();
try
{
// width
double width = 3.0;
if (ai.InitialGeneric is ArbiterWaypoint)
{
ArbiterWaypoint aw = (ArbiterWaypoint)ai.InitialGeneric;
width = width < aw.Lane.Width ? aw.Lane.Width : width;
}
if (ai.FinalGeneric is ArbiterWaypoint)
{
ArbiterWaypoint aw = (ArbiterWaypoint)ai.FinalGeneric;
width = width < aw.Lane.Width ? aw.Lane.Width : width;
}
if (ai.TurnDirection == ArbiterTurnDirection.UTurn ||
ai.TurnDirection == ArbiterTurnDirection.Straight ||
!(ai.InitialGeneric is ArbiterWaypoint) ||
!(ai.FinalGeneric is ArbiterWaypoint))
{
LinePath lp = ai.InterconnectPath.ShiftLateral(width / 2.0);
LinePath rp = ai.InterconnectPath.ShiftLateral(-width / 2.0);
polyPoints.AddRange(lp);
polyPoints.AddRange(rp);
ai.TurnPolygon = Polygon.GrahamScan(polyPoints);
if (ai.TurnDirection == ArbiterTurnDirection.UTurn)
{
List<Coordinates> updatedPts = new List<Coordinates>();
LinePath interTmp = ai.InterconnectPath.Clone();
Coordinates pathVec = ai.FinalGeneric.Position - ai.InitialGeneric.Position;
interTmp[1] = interTmp[1] + pathVec.Normalize(width / 2.0);
interTmp[0] = interTmp[0] - pathVec.Normalize(width / 2.0);
lp = interTmp.ShiftLateral(TahoeParams.VL);
rp = interTmp.ShiftLateral(-TahoeParams.VL);
updatedPts.AddRange(lp);
updatedPts.AddRange(rp);
ai.TurnPolygon = Polygon.GrahamScan(updatedPts);
}
}
else
{
// polygon points
List<Coordinates> interPoints = new List<Coordinates>();
// waypoint
ArbiterWaypoint awI = (ArbiterWaypoint)ai.InitialGeneric;
ArbiterWaypoint awF = (ArbiterWaypoint)ai.FinalGeneric;
// left and right path
LinePath leftPath = new LinePath();
LinePath rightPath = new LinePath();
// some initial points
LinePath initialPath = new LinePath(new Coordinates[] { awI.PreviousPartition.Initial.Position, awI.Position });
LinePath il = initialPath.ShiftLateral(width / 2.0);
LinePath ir = initialPath.ShiftLateral(-width / 2.0);
leftPath.Add(il[1]);
rightPath.Add(ir[1]);
// some final points
LinePath finalPath = new LinePath(new Coordinates[] { awF.Position, awF.NextPartition.Final.Position });
LinePath fl = finalPath.ShiftLateral(width / 2.0);
LinePath fr = finalPath.ShiftLateral(-width / 2.0);
leftPath.Add(fl[0]);
rightPath.Add(fr[0]);
// initial and final paths
Line iPath = new Line(awI.PreviousPartition.Initial.Position, awI.Position);
Line fPath = new Line(awF.Position, awF.NextPartition.Final.Position);
// get where the paths intersect and vector to normal path
Coordinates c;
iPath.Intersect(fPath, out c);
Coordinates vector = ai.InterconnectPath.GetClosestPoint(c).Location - c;
Coordinates center = c + vector.Normalize((vector.Length / 2.0));
// get width expansion
Coordinates iVec = awI.PreviousPartition != null ? awI.PreviousPartition.Vector().Normalize(1.0) : awI.NextPartition.Vector().Normalize(1.0);
double iRot = -iVec.ArcTan;
Coordinates fVec = awF.NextPartition != null ? awF.NextPartition.Vector().Normalize(1.0) : awF.PreviousPartition.Vector().Normalize(1.0);
fVec = fVec.Rotate(iRot);
double fDeg = fVec.ToDegrees();
double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI;
double centerWidth = width + width * 2.0 * Math.Abs(arcTan) / 90.0;
// get inner point (small scale)
Coordinates innerPoint = center + vector.Normalize(centerWidth / 4.0);
// get outer
Coordinates outerPoint = center - vector.Normalize(centerWidth / 2.0);
if (ai.TurnDirection == ArbiterTurnDirection.Right)
{
rightPath.Insert(1, innerPoint);
ai.InnerCoordinates = rightPath;
leftPath.Reverse();
leftPath.Insert(1, outerPoint);
Polygon p = new Polygon(leftPath.ToArray());
p.AddRange(rightPath.ToArray());
ai.TurnPolygon = p;
}
else
{
leftPath.Insert(1, innerPoint);
ai.InnerCoordinates = leftPath;
rightPath.Reverse();
rightPath.Insert(1, outerPoint);
Polygon p = new Polygon(leftPath.ToArray());
p.AddRange(rightPath.ToArray());
ai.TurnPolygon = p;
}
}
}
catch (Exception e)
{
Console.WriteLine("error generating turn polygon: " + ai.ToString());
ai.TurnPolygon = ai.DefaultPoly();
}
}
/// <summary>
/// Try to plan the intersection heavily penalizing the interconnect
/// </summary>
/// <param name="iTraversableWaypoint"></param>
/// <param name="iArbiterWaypoint"></param>
/// <param name="arbiterInterconnect"></param>
/// <returns></returns>
public IntersectionPlan PlanIntersectionWithoutInterconnect(ITraversableWaypoint exit, IArbiterWaypoint goal, ArbiterInterconnect interconnect)
{
// save old blockage
NavigationBlockage tmpBlockage = interconnect.Blockage;
// create new
NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);
newerBlockage.BlockageExists = true;
interconnect.Blockage = newerBlockage;
KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> > tmpCurrentTimes = currentTimes;
this.currentTimes = new KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> >();
// plan
IntersectionPlan ip = this.PlanIntersection(exit, goal);
this.currentTimes = tmpCurrentTimes;
// reset interconnect blockage
interconnect.Blockage = tmpBlockage;
// return plan
return(ip);
}
/// <summary>
/// Turn information
/// </summary>
/// <param name="entry"></param>
/// <param name="finalPath"></param>
/// <param name="leftBound"></param>
/// <param name="rightBound"></param>
public static void ZoneTurnInfo(ArbiterInterconnect ai, ArbiterPerimeterWaypoint entry, out LinePath finalPath, out LineList leftBound, out LineList rightBound)
{
//Coordinates centerVec = entry.Perimeter.PerimeterPolygon.CalculateBoundingCircle().center - entry.Position;
Coordinates centerVec = ai.InterconnectPath[1] - ai.InterconnectPath[0];
centerVec = centerVec.Normalize(TahoeParams.VL);
finalPath = new LinePath(new Coordinates[] { entry.Position, entry.Position + centerVec });
leftBound = finalPath.ShiftLateral(TahoeParams.T * 2.0);
rightBound = finalPath.ShiftLateral(-TahoeParams.T * 2.0);
}
/// <summary>
/// Gets parameters for following forward vehicle
/// </summary>
/// <param name="vehicleState"></param>
/// <param name="fullPath"></param>
/// <param name="followingSpeed"></param>
/// <param name="distanceToGood"></param>
public void Follow(VehicleState state, LinePath fullPath, ArbiterLane lane, ArbiterInterconnect turn,
out double followingSpeed, out double distanceToGood, out double xSep)
{
// get the maximum velocity of the segment we're closest to
double segV = Math.Min(lane.CurrentMaximumSpeed(state.Front), turn.MaximumDefaultSpeed);
double segMaxV = segV;
// minimum distance
double xAbsMin = TahoeParams.VL * 1.5;
// retrieve the tracked vehicle's scalar absolute speed
double vTarget = CurrentVehicle.StateMonitor.Observed.isStopped ? 0.0 : this.CurrentVehicle.Speed;
// get the good distance behind the target, is xAbsMin if vehicle velocity is small enough
double xGood = xAbsMin + (1.5 * (TahoeParams.VL / 4.4704) * vTarget);
// get our current separation
double xSeparation = state.Front.DistanceTo(this.CurrentVehicle.ClosestPosition);//lane.DistanceBetween(state.Front, this.CurrentVehicle.ClosestPosition);
xSep = xSeparation;
// determine the envelope to reason about the vehicle, that is, to slow from vMax to vehicle speed
double xEnvelope = (Math.Pow(vTarget, 2.0) - Math.Pow(segMaxV, 2.0)) / (2.0 * -0.5);
// the distance to the good
double xDistanceToGood;
// determine the velocity to follow the vehicle ahead
double vFollowing;
// check if we are basically stopped in the right place behind another vehicle
if (vTarget < 1 && Math.Abs(xSeparation - xGood) < 1)
{
// stop
vFollowing = 0;
// good distance
xDistanceToGood = 0;
}
// check if we are within the minimum distance
else if (xSeparation <= xAbsMin)
{
// stop
vFollowing = 0;
// good distance
xDistanceToGood = 0;
}
// determine if our separation is less than the good distance but not inside minimum
else if (xAbsMin < xSeparation && xSeparation < xGood)
{
// get the distance we are from xMin
double xAlong = xSeparation - xAbsMin;
// get the total distance from xGood to xMin
double xGoodToMin = xGood - xAbsMin;
// slow to 0 by min
vFollowing = (((xGoodToMin - xAlong) / xGoodToMin) * (-vTarget)) + vTarget;
// good distance
xDistanceToGood = 0;
}
// our separation is greater than the good distance but within the envelope
else if (xGood <= xSeparation && xSeparation <= xEnvelope + xGood)
{
// get differences in max and target velocities
double vDifference = Math.Max(segMaxV - vTarget, 0);
// get the distance we are along the speed envolope from xGood
double xAlong = xEnvelope - (xSeparation - xGood);
// slow to vTarget by good
vFollowing = (((xEnvelope - xAlong) / xEnvelope) * (vDifference)) + vTarget;
// good distance
xDistanceToGood = xSeparation - xGood;
}
// otherwise xSeparation > xEnvelope
else
{
// can go max speed
vFollowing = segMaxV;
// good distance
xDistanceToGood = xSeparation - xGood;
}
// set out params
followingSpeed = vFollowing;
distanceToGood = xDistanceToGood;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="turn"></param>
/// <param name="entryMontitor"></param>
public TurnForwardMonitor(ArbiterInterconnect turn, IEntryAreaMonitor entryMonitor)
{
this.turn = turn;
this.EntryMonitor = entryMonitor;
this.VehiclesToIgnore = new List<int>();
}
/// <summary>
/// Check for all waypoints who have exit interconnects that overlaps input and no stop
/// </summary>
/// <param name="exits"></param>
/// <param name="ai"></param>
/// <returns></returns>
private List<IntersectionInvolved> nonStopOverlaps(IEnumerable<ITraversableWaypoint> exits, ArbiterInterconnect ai)
{
// list of exits that have an interconnect which overlaps the interconnect input
List<IntersectionInvolved> nonStopOverlapWaypoints = new List<IntersectionInvolved>();
// get line of the interconnect
Line aiLine = new Line(ai.InitialGeneric.Position, ai.FinalGeneric.Position);
// loop over all exits
foreach (ITraversableWaypoint exit in exits)
{
// make sure not our exit and the exit is not a stop and if exit and other are both waypoints then ways not the same
if (!exit.Equals(ai.InitialGeneric) && !exit.IsStop &&
((!(ai.InitialGeneric is ArbiterWaypoint) || !(exit is ArbiterWaypoint))
|| !((ArbiterWaypoint)ai.InitialGeneric).Lane.Way.Equals(((ArbiterWaypoint)exit).Lane.Way)))
{
// get all interconnects of the exit
foreach (ArbiterInterconnect tmp in exit.Exits)
{
// check relative priority that these are equal or lesser priority
if (ai.ComparePriority(tmp) != -1)
{
// simple check if the interconnect's final is same as input final
if (tmp.FinalGeneric.Equals(ai.FinalGeneric))
{
// check not already added
if (!nonStopOverlapWaypoints.Contains(new IntersectionInvolved(((ITraversableWaypoint)tmp.FinalGeneric).VehicleArea)))
{
// add exit
nonStopOverlapWaypoints.Add(new IntersectionInvolved(exit, exit.VehicleArea, tmp.TurnDirection));
}
}
// otherwise check overlap of interconnects
else
{
// get line of tmp interconnect
Line tmpLine = new Line(tmp.InitialGeneric.Position, tmp.FinalGeneric.Position);
// position of cross
Coordinates intersectionPoint;
// check intersection
bool intersects = aiLine.Intersect(tmpLine, out intersectionPoint) && ai.IsInside(intersectionPoint);
if (intersects)
{
// check not already added
if (!nonStopOverlapWaypoints.Contains(new IntersectionInvolved(((ITraversableWaypoint)tmp.FinalGeneric).VehicleArea)))
{
// add exit
nonStopOverlapWaypoints.Add(new IntersectionInvolved(exit, exit.VehicleArea, tmp.TurnDirection));
}
}
}
}
}
}
}
return nonStopOverlapWaypoints;
}
public void SetTurnDirection(ArbiterInterconnect ai)
{
#region Turn Direction
if (ai.InitialGeneric is ArbiterWaypoint && ai.FinalGeneric is ArbiterWaypoint)
{
ArbiterWaypoint initWp = (ArbiterWaypoint)ai.InitialGeneric;
ArbiterWaypoint finWp = (ArbiterWaypoint)ai.FinalGeneric;
// check not uturn
if (!initWp.Lane.Way.Segment.Equals(finWp.Lane.Way.Segment) || initWp.Lane.Way.Equals(finWp.Lane.Way))
{
Coordinates iVec = initWp.PreviousPartition != null ? initWp.PreviousPartition.Vector().Normalize(1.0) : initWp.NextPartition.Vector().Normalize(1.0);
double iRot = -iVec.ArcTan;
Coordinates fVec = finWp.NextPartition != null ? finWp.NextPartition.Vector().Normalize(1.0) : finWp.PreviousPartition.Vector().Normalize(1.0);
fVec = fVec.Rotate(iRot);
double fDeg = fVec.ToDegrees();
double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI;
if (arcTan > 45.0)
ai.TurnDirection = ArbiterTurnDirection.Left;
else if (arcTan < -45.0)
ai.TurnDirection = ArbiterTurnDirection.Right;
else
ai.TurnDirection = ArbiterTurnDirection.Straight;
}
else
{
Coordinates iVec = initWp.PreviousPartition != null ? initWp.PreviousPartition.Vector().Normalize(1.0) : initWp.NextPartition.Vector().Normalize(1.0);
double iRot = -iVec.ArcTan;
Coordinates fVec = finWp.NextPartition != null ? finWp.NextPartition.Vector().Normalize(1.0) : finWp.PreviousPartition.Vector().Normalize(1.0);
fVec = fVec.Rotate(iRot);
double fDeg = fVec.ToDegrees();
double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI;
if (arcTan > 45.0 && arcTan < 135.0)
ai.TurnDirection = ArbiterTurnDirection.Left;
else if (arcTan < -45.0 && arcTan > -135.0)
ai.TurnDirection = ArbiterTurnDirection.Right;
else if (Math.Abs(arcTan) < 45.0)
ai.TurnDirection = ArbiterTurnDirection.Straight;
else
ai.TurnDirection = ArbiterTurnDirection.UTurn;
}
}
else
{
Coordinates iVec = new Coordinates();
double iRot = 0.0;
Coordinates fVec = new Coordinates();
double fDeg = 0.0;
if (ai.InitialGeneric is ArbiterWaypoint)
{
ArbiterWaypoint initWp = (ArbiterWaypoint)ai.InitialGeneric;
iVec = initWp.PreviousPartition != null ? initWp.PreviousPartition.Vector().Normalize(1.0) : initWp.NextPartition.Vector().Normalize(1.0);
iRot = -iVec.ArcTan;
}
else if (ai.InitialGeneric is ArbiterPerimeterWaypoint)
{
ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)ai.InitialGeneric;
Coordinates centerPoly = apw.Perimeter.PerimeterPolygon.CalculateBoundingCircle().center;
iVec = apw.Position - centerPoly;
iVec = iVec.Normalize(1.0);
iRot = -iVec.ArcTan;
}
if (ai.FinalGeneric is ArbiterWaypoint)
{
ArbiterWaypoint finWp = (ArbiterWaypoint)ai.FinalGeneric;
fVec = finWp.NextPartition != null ? finWp.NextPartition.Vector().Normalize(1.0) : finWp.PreviousPartition.Vector().Normalize(1.0);
fVec = fVec.Rotate(iRot);
fDeg = fVec.ToDegrees();
}
else if (ai.FinalGeneric is ArbiterPerimeterWaypoint)
{
ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)ai.FinalGeneric;
Coordinates centerPoly = apw.Perimeter.PerimeterPolygon.CalculateBoundingCircle().center;
fVec = centerPoly - apw.Position;
fVec = fVec.Normalize(1.0);
fVec = fVec.Rotate(iRot);
fDeg = fVec.ToDegrees();
}
double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI;
if (arcTan > 45.0)
ai.TurnDirection = ArbiterTurnDirection.Left;
else if (arcTan < -45.0)
ai.TurnDirection = ArbiterTurnDirection.Right;
else
ai.TurnDirection = ArbiterTurnDirection.Straight;
}
#endregion
}
/// <summary>
/// Determine the involved lanes with any interconnect
/// </summary>
/// <param name="exits"></param>
/// <param name="incoming"></param>
/// <returns></returns>
/// <remarks>TODO: implement</remarks>
private Dictionary<ArbiterInterconnect, List<IntersectionInvolved>> DetermineInvolved(IEnumerable<ITraversableWaypoint> exits, Dictionary<ArbiterLane, LinePath.PointOnPath> incoming)
{
// final mapping of interconnects to priority lanes that list of priority areas over each interconnect
Dictionary<ArbiterInterconnect, List<IntersectionInvolved>> priority = new Dictionary<ArbiterInterconnect, List<IntersectionInvolved>>();
// 1. Get list of all lanes incoming to the intersection
List<ArbiterLane> als = new List<ArbiterLane>();
foreach (ArbiterLane al in incoming.Keys)
{
als.Add(al);
}
// 2. Get all exits for each area
Dictionary<IVehicleArea, List<ITraversableWaypoint>> exitLookup = new Dictionary<IVehicleArea, List<ITraversableWaypoint>>();
foreach (ITraversableWaypoint aw in exits)
{
if (exitLookup.ContainsKey(aw.VehicleArea))
{
exitLookup[aw.VehicleArea].Add(aw);
}
else
{
// add all exits
List<ITraversableWaypoint> laneExits = new List<ITraversableWaypoint>();
laneExits.Add(aw);
exitLookup.Add(aw.VehicleArea, laneExits);
}
}
// 3. loop through exits and determine priority areas above them
foreach (ITraversableWaypoint aw in exits)
{
/*if (aw is ArbiterWaypoint && ((ArbiterWaypoint)aw).WaypointId.Equals(new ArbiterWaypointId(19, new ArbiterLaneId(2, new ArbiterWayId(2, new ArbiterSegmentId(6))))))
{
Console.WriteLine("");
}*/
// 3.1 loop through interconnects from exits
foreach (ArbiterInterconnect ai in aw.Exits)
{
// exit priority areas
List<IntersectionInvolved> priorityAreas = new List<IntersectionInvolved>();
// add explicit interconnects
priorityAreas.AddRange(this.laneOverlaps(als, ai, exits));
// implicit intersections
List<IntersectionInvolved> implicitInvolved = this.nonStopOverlaps(exits, ai);
foreach (IntersectionInvolved ii in implicitInvolved)
{
// make sure not contained already
if (!priorityAreas.Contains(ii))
{
// add
priorityAreas.Add(ii);
}
// if already contained, replace
else if(ii.CompareTo(priorityAreas[priorityAreas.IndexOf(ii)]) == -1)
{
priorityAreas.Remove(ii);
priorityAreas.Add(ii);
}
}
// add the priority overlaps to the exits
priority.Add(ai, priorityAreas);
}
// 3.2 check continuation if exists
if(aw.IsStop)
{
// get waypoint
ArbiterWaypoint waypoint = (ArbiterWaypoint)aw;
// check if next partition exists
if(waypoint.NextPartition != null)
{
// exit priority areas
List<IntersectionInvolved> priorityAreas = new List<IntersectionInvolved>();
// fake interconnect
ArbiterInterconnect fakeAi = new ArbiterInterconnect(waypoint, waypoint.NextPartition.Final, ArbiterTurnDirection.Straight);
#region New
// list of next intersection ivolved
List<IntersectionInvolved> nextII = new List<IntersectionInvolved>();
// list of fake interconnects defining the continuation
List<ArbiterInterconnect> fakeAis = new List<ArbiterInterconnect>();
// add the next partition by default
fakeAis.Add(fakeAi);
// entries into lane of fake ai involved in the intersection
List<ArbiterWaypoint> laneEntries = new List<ArbiterWaypoint>();
foreach (ITraversableWaypoint itw in exits)
{
foreach (ArbiterInterconnect aiTmp in itw.Exits)
{
if (aiTmp.FinalGeneric is ArbiterWaypoint)
{
ArbiterWaypoint awTmp = (ArbiterWaypoint)aiTmp.FinalGeneric;
if (awTmp.Lane.Equals(waypoint.Lane) && !awTmp.Equals(waypoint.NextPartition.Final) && !laneEntries.Contains(awTmp))
{
ArbiterInterconnect tmpFake = new ArbiterInterconnect(waypoint, awTmp, ArbiterTurnDirection.Straight);
if (!fakeAis.Contains(tmpFake))
{
laneEntries.Add(awTmp);
fakeAis.Add(tmpFake);
}
}
}
}
}
// loop through fake ais adding ii
foreach (ArbiterInterconnect fake in fakeAis)
{
// explicit and explicit add to tmp
List<IntersectionInvolved> tmpIis = this.laneOverlaps(als, fake, exits);
tmpIis.AddRange(this.nonStopOverlaps(exits, fake));
// check and add
foreach (IntersectionInvolved tmpIi in tmpIis)
{
if (!nextII.Contains(tmpIi))
nextII.Add(tmpIi);
else if (nextII[nextII.IndexOf(tmpIi)].Exit == null)
{
nextII.Remove(tmpIi);
nextII.Add(tmpIi);
}
}
}
// add to priority areas
priorityAreas.AddRange(nextII);
#endregion
#region Old
/*
// fake interconnect
ArbiterInterconnect fakeAi = new ArbiterInterconnect(waypoint, waypoint.NextPartition.Final);
// add explicit interconnects
priorityAreas.AddRange(this.laneOverlaps(als, fakeAi));
// add implicit interconnects
priorityAreas.AddRange(this.nonStopOverlaps(exits, fakeAi));
// add the priority overlaps to the exits
priority.Add(fakeAi, priorityAreas);
*/
#endregion
// add the priority overlaps to the exits
if (priority.ContainsKey(fakeAi))
EditorOutput.WriteLine("Error adding interconnect: " + fakeAi.ToString() + " to priority areas as key already existed, check priorities");
else
priority.Add(fakeAi, priorityAreas);
}
}
}
// return the final priorities
return priority;
}
/// <summary>
/// Try to plan the intersection heavily penalizing the interconnect
/// </summary>
/// <param name="iTraversableWaypoint"></param>
/// <param name="iArbiterWaypoint"></param>
/// <param name="arbiterInterconnect"></param>
/// <returns></returns>
public IntersectionPlan PlanIntersectionWithoutInterconnect(ITraversableWaypoint exit, IArbiterWaypoint goal, ArbiterInterconnect interconnect)
{
// save old blockage
NavigationBlockage tmpBlockage = interconnect.Blockage;
// create new
NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);
newerBlockage.BlockageExists = true;
interconnect.Blockage = newerBlockage;
KeyValuePair<int, Dictionary<ArbiterWaypointId, DownstreamPointOfInterest>> tmpCurrentTimes = currentTimes;
this.currentTimes = new KeyValuePair<int, Dictionary<ArbiterWaypointId, DownstreamPointOfInterest>>();
// plan
IntersectionPlan ip = this.PlanIntersection(exit, goal);
this.currentTimes = tmpCurrentTimes;
// reset interconnect blockage
interconnect.Blockage = tmpBlockage;
// return plan
return ip;
}
/// <summary>
/// Gets primary maneuver given our position and the turn we are traveling upon
/// </summary>
/// <param name="vehicleState"></param>
/// <returns></returns>
public Maneuver PrimaryManeuver(VehicleState vehicleState, List <ITacticalBlockage> blockages, TurnState turnState)
{
#region Check are planning over the correct turn
if (CoreCommon.CorePlanningState is TurnState)
{
TurnState ts = (TurnState)CoreCommon.CorePlanningState;
if (this.turn == null || !this.turn.Equals(ts.Interconnect))
{
this.turn = ts.Interconnect;
this.forwardMonitor = new TurnForwardMonitor(ts.Interconnect, null);
}
else if (this.forwardMonitor.turn == null || !this.forwardMonitor.turn.Equals(ts.Interconnect))
{
this.forwardMonitor = new TurnForwardMonitor(ts.Interconnect, null);
}
}
#endregion
#region Blockages
// check blockages
if (blockages != null && blockages.Count > 0 && blockages[0] is TurnBlockage)
{
// create the blockage state
EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);
// check not at highest level already
if (turnState.Saudi != SAUDILevel.L1 || turnState.UseTurnBounds)
{
// check not from a dynamicly moving vehicle
if (blockages[0].BlockageReport.BlockageType != BlockageType.Dynamic ||
(TacticalDirector.ValidVehicles.ContainsKey(blockages[0].BlockageReport.TrackID) &&
TacticalDirector.ValidVehicles[blockages[0].BlockageReport.TrackID].IsStopped))
{
// go to a blockage handling tactical
return(new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
else
{
ArbiterOutput.Output("Turn blockage reported for moving vehicle, ignoring");
}
}
else
{
ArbiterOutput.Output("Turn blockage, but recovery escalation already at highest state, ignoring report");
}
}
#endregion
#region Intersection Check
if (!this.CanGo(vehicleState))
{
if (turn.FinalGeneric is ArbiterWaypoint)
{
TravelingParameters tp = this.GetParameters(0.0, 0.0, (ArbiterWaypoint)turn.FinalGeneric, vehicleState, false);
return(new Maneuver(tp.Behavior, CoreCommon.CorePlanningState, tp.NextState.DefaultStateDecorators, vehicleState.Timestamp));
}
else
{
// get turn params
LinePath finalPath;
LineList leftLL;
LineList rightLL;
IntersectionToolkit.ZoneTurnInfo(this.turn, (ArbiterPerimeterWaypoint)this.turn.FinalGeneric, out finalPath, out leftLL, out rightLL);
// hold brake
IState nextState = new TurnState(this.turn, this.turn.TurnDirection, null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(0.0));
TurnBehavior b = new TurnBehavior(null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(0.0), this.turn.InterconnectId);
return(new Maneuver(b, CoreCommon.CorePlanningState, nextState.DefaultStateDecorators, vehicleState.Timestamp));
}
}
#endregion
#region Final is Lane Waypoint
if (turn.FinalGeneric is ArbiterWaypoint)
{
// final point
ArbiterWaypoint final = (ArbiterWaypoint)turn.FinalGeneric;
// plan down entry lane
RoadPlan rp = navigation.PlanNavigableArea(final.Lane, final.Position,
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId], new List <ArbiterWaypoint>());
// point of interest downstream
DownstreamPointOfInterest dpoi = rp.BestPlan.laneWaypointOfInterest;
// get path this represents
List <Coordinates> pathCoordinates = new List <Coordinates>();
pathCoordinates.Add(vehicleState.Position);
foreach (ArbiterWaypoint aw in final.Lane.WaypointsInclusive(final, final.Lane.WaypointList[final.Lane.WaypointList.Count - 1]))
{
pathCoordinates.Add(aw.Position);
}
LinePath lp = new LinePath(pathCoordinates);
// list of all parameterizations
List <TravelingParameters> parameterizations = new List <TravelingParameters>();
// get lane navigation parameterization
TravelingParameters navigationParameters = this.NavigationParameterization(vehicleState, dpoi, final, lp);
parameterizations.Add(navigationParameters);
// update forward tracker and get vehicle parameterizations if forward vehicle exists
this.forwardMonitor.Update(vehicleState, final, lp);
if (this.forwardMonitor.ShouldUseForwardTracker())
{
// get vehicle parameterization
TravelingParameters vehicleParameters = this.VehicleParameterization(vehicleState, lp, final);
parameterizations.Add(vehicleParameters);
}
// sort and return funal
parameterizations.Sort();
// get the final behavior
TurnBehavior tb = (TurnBehavior)parameterizations[0].Behavior;
// get vehicles to ignore
tb.VehiclesToIgnore = this.forwardMonitor.VehiclesToIgnore;
// add persistent information about saudi level
if (turnState.Saudi == SAUDILevel.L1)
{
tb.Decorators = new List <BehaviorDecorator>(tb.Decorators.ToArray());
tb.Decorators.Add(new ShutUpAndDoItDecorator(SAUDILevel.L1));
}
// add persistent information about turn bounds
if (!turnState.UseTurnBounds)
{
tb.LeftBound = null;
tb.RightBound = null;
}
// return the behavior
return(new Maneuver(tb, CoreCommon.CorePlanningState, tb.Decorators, vehicleState.Timestamp));
}
#endregion
#region Final is Zone Waypoint
else if (turn.FinalGeneric is ArbiterPerimeterWaypoint)
{
// get inteconnect path
Coordinates entryVec = ((ArbiterPerimeterWaypoint)turn.FinalGeneric).Perimeter.PerimeterPolygon.BoundingCircle.center -
turn.FinalGeneric.Position;
entryVec = entryVec.Normalize(TahoeParams.VL / 2.0);
LinePath ip = new LinePath(new Coordinates[] { turn.InitialGeneric.Position, turn.FinalGeneric.Position, entryVec + this.turn.FinalGeneric.Position });
// get distance from end
double d = ip.DistanceBetween(
ip.GetClosestPoint(vehicleState.Front),
ip.EndPoint);
// get speed command
SpeedCommand sc = null;
if (d < TahoeParams.VL)
{
sc = new StopAtDistSpeedCommand(d);
}
else
{
sc = new ScalarSpeedCommand(SpeedTools.GenerateSpeed(d - TahoeParams.VL, 1.7, turn.MaximumDefaultSpeed));
}
// final perimeter waypoint
ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)this.turn.FinalGeneric;
// get turn params
LinePath finalPath;
LineList leftLL;
LineList rightLL;
IntersectionToolkit.ZoneTurnInfo(this.turn, (ArbiterPerimeterWaypoint)this.turn.FinalGeneric, out finalPath, out leftLL, out rightLL);
// hold brake
IState nextState = new TurnState(this.turn, this.turn.TurnDirection, null, finalPath, leftLL, rightLL, sc);
TurnBehavior tb = new TurnBehavior(null, finalPath, leftLL, rightLL, sc, null, new List <int>(), this.turn.InterconnectId);
// add persistent information about saudi level
if (turnState.Saudi == SAUDILevel.L1)
{
tb.Decorators = new List <BehaviorDecorator>(tb.Decorators.ToArray());
tb.Decorators.Add(new ShutUpAndDoItDecorator(SAUDILevel.L1));
}
// add persistent information about turn bounds
if (!turnState.UseTurnBounds)
{
tb.LeftBound = null;
tb.RightBound = null;
}
// return maneuver
return(new Maneuver(tb, CoreCommon.CorePlanningState, tb.Decorators, vehicleState.Timestamp));
}
#endregion
#region Unknown
else
{
throw new Exception("unrecognized type: " + turn.FinalGeneric.ToString());
}
#endregion
}
/// <summary>
/// Try to plan the intersection heavily penalizing the interconnect
/// </summary>
/// <param name="iTraversableWaypoint"></param>
/// <param name="iArbiterWaypoint"></param>
/// <param name="arbiterInterconnect"></param>
/// <returns></returns>
public IntersectionPlan PlanIntersectionWithoutInterconnect(ITraversableWaypoint exit, IArbiterWaypoint goal, ArbiterInterconnect interconnect, bool blockAllRelated)
{
ITraversableWaypoint entry = (ITraversableWaypoint)interconnect.FinalGeneric;
if (!blockAllRelated)
return this.PlanIntersectionWithoutInterconnect(exit, goal, interconnect);
else
{
Dictionary<IConnectAreaWaypoints, NavigationBlockage> saved = new Dictionary<IConnectAreaWaypoints, NavigationBlockage>();
if (entry.IsEntry)
{
foreach (ArbiterInterconnect ai in entry.Entries)
{
saved.Add(ai, ai.Blockage);
// create new
NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);
newerBlockage.BlockageExists = true;
ai.Blockage = newerBlockage;
}
}
if (entry is ArbiterWaypoint && ((ArbiterWaypoint)entry).PreviousPartition != null)
{
ArbiterLanePartition alp = ((ArbiterWaypoint)entry).PreviousPartition;
saved.Add(alp, alp.Blockage);
// create new
NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);
newerBlockage.BlockageExists = true;
alp.Blockage = newerBlockage;
}
KeyValuePair<int, Dictionary<ArbiterWaypointId, DownstreamPointOfInterest>> tmpCurrentTimes = currentTimes;
this.currentTimes = new KeyValuePair<int, Dictionary<ArbiterWaypointId, DownstreamPointOfInterest>>();
// plan
IntersectionPlan ip = this.PlanIntersection(exit, goal);
this.currentTimes = tmpCurrentTimes;
// reset the blockages
foreach (KeyValuePair<IConnectAreaWaypoints, NavigationBlockage> savedPair in saved)
savedPair.Key.Blockage = savedPair.Value;
// return plan
return ip;
}
}
/// <summary>
/// Makes polygon representing the entry
/// </summary>
public Polygon GenerateEntryMonitorPolygon(ArbiterInterconnect ai)
{
Coordinates aiVector = ai.FinalGeneric.Position - ai.InitialGeneric.Position;
Coordinates aiEntry = this.finalWaypoint.Position + aiVector.Normalize(TahoeParams.VL * 1.5);
Coordinates centerVector = this.finalWaypoint.Perimeter.PerimeterPolygon.CalculateBoundingCircle().center - this.finalWaypoint.Position;
Coordinates centerEntry = this.finalWaypoint.Position + centerVector.Normalize(TahoeParams.VL * 1.5);
List<Coordinates> boundingCoords = new List<Coordinates>(new Coordinates[] { aiEntry, centerEntry, finalWaypoint.Position });
return Polygon.GrahamScan(boundingCoords).Inflate(2.0 * TahoeParams.T);
}
/// <summary>
/// Resets the interconnect we wish to take
/// </summary>
/// <param name="reset"></param>
public void ResetDesired(ArbiterInterconnect desired)
{
// create monitors
this.AllMonitors = this.IntersectionPriorityQueue;
this.NonStopPriorityAreas = new List <IDominantMonitor>();
this.PreviouslyWaiting = new List <IDominantMonitor>();
this.cooldownTimer = new Stopwatch();
#region Priority Areas Over Interconnect
// check contains priority lane for this
if (this.Intersection.PriorityLanes.ContainsKey(desired.ToInterconnect))
{
// loop through all other priority monitors over this interconnect
foreach (IntersectionInvolved ii in this.Intersection.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(this.OurMonitor.Waypoint, ((ArbiterPerimeterWaypoint)desired.FinalGeneric).Perimeter.Zone,
((ArbiterInterconnect)desired).TurnDirection));
}
else
{
throw new Exception("unhandled desired interconnect final type");
}
#endregion
// add to all
foreach (IIntersectionQueueable iiq in this.NonStopPriorityAreas)
{
this.AllMonitors.Add(iiq);
}
this.AllMonitors.Add(this.EntryAreaMonitor);
// update all
this.Update(CoreCommon.Communications.GetVehicleState());
}
/// <summary>
/// Maneuver for recovering from a turn blockage
/// </summary>
/// <param name="lane"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicleSpeed"></param>
/// <param name="defcon"></param>
/// <param name="saudi"></param>
/// <returns></returns>
private Maneuver TurnRecoveryManeuver(ArbiterInterconnect ai, VehicleState vehicleState,
double vehicleSpeed, BlockageRecoveryState brs)
{
// get the blockage
ITacticalBlockage turnBlockageReport = brs.EncounteredState.TacticalBlockage;
// get the turn state
TurnState turnState = (TurnState)brs.EncounteredState.PlanningState;
// check the state of the recovery for being the initial state
if (brs.Defcon == BlockageRecoveryDEFCON.INITIAL)
{
// determine if the reverse behavior is recommended
if (turnBlockageReport.BlockageReport.ReverseRecommended)
{
// get the path of the interconnect
LinePath interconnectPath = ai.InterconnectPath;
// check how much room there is to the start of the interconnect
double distanceToStart = interconnectPath.DistanceBetween(interconnectPath.StartPoint, interconnectPath.GetClosestPoint(vehicleState.Rear));
// check distance to start from the rear bumper is large enough
if (distanceToStart > 0)
{
// notify
ArbiterOutput.Output("Initial encounter of turn blockage with reverse recommended, reversing");
// get hte reverse path
LinePath reversePath = vehicleState.VehicleLinePath;
// generate the reverse recovery behavior
StopAtDistSpeedCommand sadsc = new StopAtDistSpeedCommand(TahoeParams.VL, true);
StayInLaneBehavior silb = new StayInLaneBehavior(null, sadsc, new List<int>(), reversePath, TahoeParams.T * 2.0, 0, 0);
// update the state
BlockageRecoveryState brsUpdated = new BlockageRecoveryState(
silb, brs.CompletionState, brs.AbortState, BlockageRecoveryDEFCON.REVERSE,
brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
// chill out
BlockageHandler.SetDefaultBlockageCooldown();
// send the maneuver
Maneuver recoveryManeuver = new Maneuver(
silb, brsUpdated, TurnDecorators.HazardDecorator, vehicleState.Timestamp);
return recoveryManeuver;
}
}
}
// get the default turn behavior
TurnBehavior defaultTurnBehavior = (TurnBehavior)turnState.Resume(vehicleState, vehicleSpeed);
// check that we are not already ignoring small obstacles
if (turnState.Saudi == SAUDILevel.None)
{
// check the turn ignoring small obstacles
ShutUpAndDoItDecorator saudiDecorator = new ShutUpAndDoItDecorator(SAUDILevel.L1);
defaultTurnBehavior.Decorators.Add(saudiDecorator);
turnState.Saudi = SAUDILevel.L1;
// notify
ArbiterOutput.Output("Attempting test of turn behavior ignoring small obstacles");
// test execute the turn
CompletionReport saudiCompletionReport;
bool completedSaudiTurn = CoreCommon.Communications.TestExecute(defaultTurnBehavior, out saudiCompletionReport);
// if the turn worked with ignorimg small obstacles, execute that
if (completedSaudiTurn)
{
// notify
ArbiterOutput.Output("Test of turn behavior ignoring small obstacles successful");
// chill out
BlockageHandler.SetDefaultBlockageCooldown();
// return the recovery maneuver
return new Maneuver(defaultTurnBehavior, turnState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
// notify
ArbiterOutput.Output("Turn behavior reached last level of using no turn boundaries");
// when ignoring small obstacles does not work, send the turn without boundaries and ignore small obstacles
turnState.UseTurnBounds = false;
turnState.LeftBound = null;
turnState.RightBound = null;
defaultTurnBehavior.RightBound = null;
defaultTurnBehavior.LeftBound = null;
// ignoring small obstacles
ShutUpAndDoItDecorator saudiNoBoundsDecorator = new ShutUpAndDoItDecorator(SAUDILevel.L1);
defaultTurnBehavior.Decorators.Add(saudiNoBoundsDecorator);
turnState.Saudi = SAUDILevel.L1;
// chill out
BlockageHandler.SetDefaultBlockageCooldown();
// go to the turn state
return new Maneuver(defaultTurnBehavior, turnState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
/// <summary>
/// Generates interconnects into the road network
/// </summary>
/// <param name="arn"></param>
/// <returns></returns>
public ArbiterRoadNetwork GenerateInterconnects(ArbiterRoadNetwork arn)
{
// list of all exit entries in the xy rndf
List<SimpleExitEntry> sees = new List<SimpleExitEntry>();
// zones
if(xyRndf.Zones != null)
{
// loop over zones
foreach (SimpleZone sz in xyRndf.Zones)
{
// add all ee's
sees.AddRange(sz.Perimeter.ExitEntries);
}
}
// segments
if (xyRndf.Segments != null)
{
// loop over segments
foreach (SimpleSegment ss in xyRndf.Segments)
{
// lanes
foreach (SimpleLane sl in ss.Lanes)
{
// add all ee's
sees.AddRange(sl.ExitEntries);
}
}
}
// loop over ee's and create interconnects
foreach (SimpleExitEntry see in sees)
{
IArbiterWaypoint initial = arn.LegacyWaypointLookup[see.ExitId];
IArbiterWaypoint final = arn.LegacyWaypointLookup[see.EntryId];
ArbiterInterconnect ai = new ArbiterInterconnect(initial, final);
arn.ArbiterInterconnects.Add(ai.InterconnectId, ai);
arn.DisplayObjects.Add(ai);
if (initial is ITraversableWaypoint)
{
ITraversableWaypoint initialWaypoint = (ITraversableWaypoint)initial;
initialWaypoint.IsExit = true;
if (initialWaypoint.Exits == null)
initialWaypoint.Exits = new List<ArbiterInterconnect>();
initialWaypoint.Exits.Add(ai);
}
else
{
throw new Exception("initial wp of ee: " + see.ExitId + " is not ITraversableWaypoint");
}
if (final is ITraversableWaypoint)
{
ITraversableWaypoint finalWaypoint = (ITraversableWaypoint)final;
finalWaypoint.IsEntry = true;
if (finalWaypoint.Entries == null)
finalWaypoint.Entries = new List<ArbiterInterconnect>();
finalWaypoint.Entries.Add(ai);
}
else
{
throw new Exception("final wp of ee: " + see.EntryId + " is not ITraversableWaypoint");
}
// set the turn direction
this.SetTurnDirection(ai);
// interconnectp olygon stuff
this.GenerateInterconnectPolygon(ai);
if (ai.TurnPolygon.IsComplex)
{
Console.WriteLine("Found complex polygon for interconnect: " + ai.ToString());
ai.TurnPolygon = ai.DefaultPoly();
}
}
return arn;
}
/// <summary>
/// Startup the vehicle from a certain position, pass the next state back,
/// and initialize the lane agent if possible
/// </summary>
/// <param name="vehicleState"></param>
/// <returns></returns>
public IState Startup(VehicleState vehicleState, CarMode carMode)
{
// check car mode
if (carMode == CarMode.Run)
{
// check areas
ArbiterLane al = CoreCommon.RoadNetwork.ClosestLane(vehicleState.Front);
ArbiterZone az = CoreCommon.RoadNetwork.InZone(vehicleState.Front);
ArbiterIntersection ain = CoreCommon.RoadNetwork.InIntersection(vehicleState.Front);
ArbiterInterconnect ai = CoreCommon.RoadNetwork.ClosestInterconnect(vehicleState.Front, vehicleState.Heading);
if (az != null)
{
// special zone startup
return(new ZoneStartupState(az));
}
if (ain != null)
{
if (al != null)
{
// check lane stuff
PointOnPath lanePoint = al.PartitionPath.GetClosest(vehicleState.Front);
// get distance from front of car
double dist = lanePoint.pt.DistanceTo(vehicleState.Front);
// check dist to lane
if (dist < al.Width + 1.0)
{
// check orientation relative to lane
Coordinates laneVector = al.GetClosestPartition(vehicleState.Front).Vector().Normalize();
// get our heading
Coordinates ourHeading = vehicleState.Heading.Normalize();
// forwards or backwards
bool forwards = true;
// check dist to each other
if (laneVector.DistanceTo(ourHeading) > Math.Sqrt(2.0))
{
// not going forwards
forwards = false;
}
// stay in lane if forwards
if (forwards)
{
ArbiterOutput.Output("Starting up in lane: " + al.ToString());
return(new StayInLaneState(al, new Probability(0.7, 0.3), true, CoreCommon.CorePlanningState));
}
else
{
// Opposing lane
return(new OpposingLanesState(al, true, CoreCommon.CorePlanningState, vehicleState));
}
}
}
// startup intersection state
return(new IntersectionStartupState(ain));
}
if (al != null)
{
// get a startup chute
ArbiterLanePartition startupChute = this.GetStartupChute(vehicleState);
// check if in a startup chute
if (startupChute != null && !startupChute.IsInside(vehicleState.Front))
{
ArbiterOutput.Output("Starting up in chute: " + startupChute.ToString());
return(new StartupOffChuteState(startupChute));
}
// not in a startup chute
else
{
PointOnPath lanePoint = al.PartitionPath.GetClosest(vehicleState.Front);
// get distance from front of car
double dist = lanePoint.pt.DistanceTo(vehicleState.Front);
// check orientation relative to lane
Coordinates laneVector = al.GetClosestPartition(vehicleState.Front).Vector().Normalize();
// get our heading
Coordinates ourHeading = vehicleState.Heading.Normalize();
// forwards or backwards
bool forwards = true;
// check dist to each other
if (laneVector.DistanceTo(ourHeading) > Math.Sqrt(2.0))
{
// not going forwards
forwards = false;
}
// stay in lane if forwards
if (forwards)
{
ArbiterOutput.Output("Starting up in lane: " + al.ToString());
return(new StayInLaneState(al, new Probability(0.7, 0.3), true, CoreCommon.CorePlanningState));
}
else
{
// opposing
return(new OpposingLanesState(al, true, CoreCommon.CorePlanningState, vehicleState));
}
}
}
// fell out
ArbiterOutput.Output("Cannot find area to startup in");
return(CoreCommon.CorePlanningState);
}
else
{
return(CoreCommon.CorePlanningState);
}
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="turn"></param>
/// <param name="entryMontitor"></param>
public TurnForwardMonitor(ArbiterInterconnect turn, IEntryAreaMonitor entryMonitor)
{
this.turn = turn;
this.EntryMonitor = entryMonitor;
this.VehiclesToIgnore = new List <int>();
}
/// <summary>
/// Add blockage to interconnect
/// </summary>
/// <param name="arbiterInterconnect"></param>
public void AddInterconnectBlockage(ArbiterInterconnect arbiterInterconnect)
{
this.AddBlockage(arbiterInterconnect, CoreCommon.Communications.GetVehicleState().Position, false);
}
private static void GenerateInterconnectPolygon(ArbiterInterconnect ai)
{
List <Coordinates> polyPoints = new List <Coordinates>();
// width
double width = 3.0;
if (ai.InitialGeneric is ArbiterWaypoint)
{
ArbiterWaypoint aw = (ArbiterWaypoint)ai.InitialGeneric;
width = width < aw.Lane.Width ? aw.Lane.Width : width;
}
if (ai.FinalGeneric is ArbiterWaypoint)
{
ArbiterWaypoint aw = (ArbiterWaypoint)ai.FinalGeneric;
width = width < aw.Lane.Width ? aw.Lane.Width : width;
}
if (ai.TurnDirection == ArbiterTurnDirection.UTurn ||
ai.TurnDirection == ArbiterTurnDirection.Straight ||
!(ai.InitialGeneric is ArbiterWaypoint) ||
!(ai.FinalGeneric is ArbiterWaypoint))
{
LinePath lp = ai.InterconnectPath.ShiftLateral(width / 2.0);
LinePath rp = ai.InterconnectPath.ShiftLateral(-width / 2.0);
polyPoints.AddRange(lp);
polyPoints.AddRange(rp);
ai.TurnPolygon = Polygon.GrahamScan(polyPoints);
if (ai.TurnDirection == ArbiterTurnDirection.UTurn)
{
List <Coordinates> updatedPts = new List <Coordinates>();
LinePath interTmp = ai.InterconnectPath.Clone();
Coordinates pathVec = ai.FinalGeneric.Position - ai.InitialGeneric.Position;
interTmp[1] = interTmp[1] + pathVec.Normalize(width / 2.0);
interTmp[0] = interTmp[0] - pathVec.Normalize(width / 2.0);
lp = interTmp.ShiftLateral(TahoeParams.VL);
rp = interTmp.ShiftLateral(-TahoeParams.VL);
updatedPts.AddRange(lp);
updatedPts.AddRange(rp);
ai.TurnPolygon = Polygon.GrahamScan(updatedPts);
}
}
else
{
// polygon points
List <Coordinates> interPoints = new List <Coordinates>();
// waypoint
ArbiterWaypoint awI = (ArbiterWaypoint)ai.InitialGeneric;
ArbiterWaypoint awF = (ArbiterWaypoint)ai.FinalGeneric;
// left and right path
LinePath leftPath = new LinePath();
LinePath rightPath = new LinePath();
// some initial points
LinePath initialPath = new LinePath(new Coordinates[] { awI.PreviousPartition.Initial.Position, awI.Position });
LinePath il = initialPath.ShiftLateral(width / 2.0);
LinePath ir = initialPath.ShiftLateral(-width / 2.0);
leftPath.Add(il[1]);
rightPath.Add(ir[1]);
// some final points
LinePath finalPath = new LinePath(new Coordinates[] { awF.Position, awF.NextPartition.Final.Position });
LinePath fl = finalPath.ShiftLateral(width / 2.0);
LinePath fr = finalPath.ShiftLateral(-width / 2.0);
leftPath.Add(fl[0]);
rightPath.Add(fr[0]);
// initial and final paths
Line iPath = new Line(awI.PreviousPartition.Initial.Position, awI.Position);
Line fPath = new Line(awF.Position, awF.NextPartition.Final.Position);
// get where the paths intersect and vector to normal path
Coordinates c;
iPath.Intersect(fPath, out c);
Coordinates vector = ai.InterconnectPath.GetClosestPoint(c).Location - c;
Coordinates center = c + vector.Normalize((vector.Length / 2.0));
// get width expansion
Coordinates iVec = awI.PreviousPartition != null?awI.PreviousPartition.Vector().Normalize(1.0) : awI.NextPartition.Vector().Normalize(1.0);
double iRot = -iVec.ArcTan;
Coordinates fVec = awF.NextPartition != null?awF.NextPartition.Vector().Normalize(1.0) : awF.PreviousPartition.Vector().Normalize(1.0);
fVec = fVec.Rotate(iRot);
double fDeg = fVec.ToDegrees();
double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI;
double centerWidth = width + width * 1.0 * Math.Abs(arcTan) / 90.0;
// get inner point (small scale)
Coordinates innerPoint = center + vector.Normalize(centerWidth / 4.0);
// get outer
Coordinates outerPoint = center - vector.Normalize(centerWidth / 2.0);
if (ai.TurnDirection == ArbiterTurnDirection.Right)
{
rightPath.Insert(1, innerPoint);
ai.InnerCoordinates = rightPath;
leftPath.Reverse();
leftPath.Insert(1, outerPoint);
Polygon p = new Polygon(leftPath.ToArray());
p.AddRange(rightPath.ToArray());
ai.TurnPolygon = p;
}
else
{
leftPath.Insert(1, innerPoint);
ai.InnerCoordinates = leftPath;
rightPath.Reverse();
rightPath.Insert(1, outerPoint);
Polygon p = new Polygon(leftPath.ToArray());
p.AddRange(rightPath.ToArray());
ai.TurnPolygon = p;
}
}
}
/// <summary>
/// Try to plan the intersection heavily penalizing the interconnect
/// </summary>
/// <param name="iTraversableWaypoint"></param>
/// <param name="iArbiterWaypoint"></param>
/// <param name="arbiterInterconnect"></param>
/// <returns></returns>
public IntersectionPlan PlanIntersectionWithoutInterconnect(ITraversableWaypoint exit, IArbiterWaypoint goal, ArbiterInterconnect interconnect, bool blockAllRelated)
{
ITraversableWaypoint entry = (ITraversableWaypoint)interconnect.FinalGeneric;
if (!blockAllRelated)
{
return(this.PlanIntersectionWithoutInterconnect(exit, goal, interconnect));
}
else
{
Dictionary <IConnectAreaWaypoints, NavigationBlockage> saved = new Dictionary <IConnectAreaWaypoints, NavigationBlockage>();
if (entry.IsEntry)
{
foreach (ArbiterInterconnect ai in entry.Entries)
{
saved.Add(ai, ai.Blockage);
// create new
NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);
newerBlockage.BlockageExists = true;
ai.Blockage = newerBlockage;
}
}
if (entry is ArbiterWaypoint && ((ArbiterWaypoint)entry).PreviousPartition != null)
{
ArbiterLanePartition alp = ((ArbiterWaypoint)entry).PreviousPartition;
saved.Add(alp, alp.Blockage);
// create new
NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);
newerBlockage.BlockageExists = true;
alp.Blockage = newerBlockage;
}
KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> > tmpCurrentTimes = currentTimes;
this.currentTimes = new KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> >();
// plan
IntersectionPlan ip = this.PlanIntersection(exit, goal);
this.currentTimes = tmpCurrentTimes;
// reset the blockages
foreach (KeyValuePair <IConnectAreaWaypoints, NavigationBlockage> savedPair in saved)
{
savedPair.Key.Blockage = savedPair.Value;
}
// return plan
return(ip);
}
}
public static Polygon PartitionPolygon(ArbiterLanePartition alp)
{
if (alp.Initial.PreviousPartition != null &&
alp.Final.NextPartition != null &&
alp.Length < 30.0 && alp.Length > 4.0)
{
// get partition turn direction
ArbiterTurnDirection pTD = PartitionTurnDirection(alp);
// check if angles of previous and next are such that not straight through
if (pTD != ArbiterTurnDirection.Straight)
{
// get partition poly
ArbiterInterconnect tmpAi = alp.ToInterconnect;
tmpAi.TurnDirection = pTD;
GenerateInterconnectPolygon(tmpAi);
Polygon pPoly = tmpAi.TurnPolygon;
// here is default partition polygon
LinePath alplb = alp.PartitionPath.ShiftLateral(-alp.Lane.Width / 2.0);
LinePath alprb = alp.PartitionPath.ShiftLateral(alp.Lane.Width / 2.0);
alprb.Reverse();
List <Coordinates> alpdefaultPoly = alplb;
alpdefaultPoly.AddRange(alprb);
// get full poly
pPoly.AddRange(alpdefaultPoly);
pPoly = Polygon.GrahamScan(pPoly);
return(pPoly);
}
}
else if (alp.Length >= 30)
{
Polygon pBase = GenerateSimplePartitionPolygon(alp, alp.PartitionPath, alp.Lane.Width);
if (alp.Initial.PreviousPartition != null && Math.Abs(FinalIntersectionAngle(alp.Initial.PreviousPartition)) > 15)
{
// initial portion
Coordinates i1 = alp.Initial.Position - alp.Initial.PreviousPartition.Vector().Normalize(15.0);
Coordinates i2 = alp.Initial.Position;
Coordinates i3 = i2 + alp.Vector().Normalize(15.0);
LinePath il12 = new LinePath(new Coordinates[] { i1, i2 });
LinePath il23 = new LinePath(new Coordinates[] { i2, i3 });
LinePath il13 = new LinePath(new Coordinates[] { i1, i3 });
Coordinates iCC = il13.GetClosestPoint(i2).Location;
if (GeneralToolkit.TriangleArea(i1, i2, i3) < 0)
{
il13 = il13.ShiftLateral(iCC.DistanceTo(i2) + alp.Lane.Width / 2.0);
}
else
{
il13 = il13.ShiftLateral(-iCC.DistanceTo(i2) + alp.Lane.Width / 2.0);
}
LinePath.PointOnPath iCCP = il13.GetClosestPoint(iCC);
iCCP = il13.AdvancePoint(iCCP, -10.0);
il13 = il13.SubPath(iCCP, 20.0);
Polygon iBase = GenerateSimplePolygon(il23, alp.Lane.Width);
iBase.Add(il13[1]);
Polygon iP = Polygon.GrahamScan(iBase);
pBase = PolygonToolkit.PolygonUnion(new List <Polygon>(new Polygon[] { pBase, iP }));
}
if (alp.Final.NextPartition != null && Math.Abs(FinalIntersectionAngle(alp)) > 15)
{
// initial portion
Coordinates i1 = alp.Final.Position - alp.Vector().Normalize(15.0);
Coordinates i2 = alp.Final.Position;
Coordinates i3 = i2 + alp.Final.NextPartition.Vector().Normalize(15.0);
LinePath il12 = new LinePath(new Coordinates[] { i1, i2 });
LinePath il23 = new LinePath(new Coordinates[] { i2, i3 });
LinePath il13 = new LinePath(new Coordinates[] { i1, i3 });
Coordinates iCC = il13.GetClosestPoint(i2).Location;
if (GeneralToolkit.TriangleArea(i1, i2, i3) < 0)
{
il13 = il13.ShiftLateral(iCC.DistanceTo(i2) + alp.Lane.Width / 2.0);
}
else
{
il13 = il13.ShiftLateral(-iCC.DistanceTo(i2) + alp.Lane.Width / 2.0);
}
LinePath.PointOnPath iCCP = il13.GetClosestPoint(iCC);
iCCP = il13.AdvancePoint(iCCP, -10.0);
il13 = il13.SubPath(iCCP, 20.0);
Polygon iBase = GenerateSimplePolygon(il12, alp.Lane.Width);
iBase.Add(il13[0]);
Polygon iP = Polygon.GrahamScan(iBase);
pBase = PolygonToolkit.PolygonUnion(new List <Polygon>(new Polygon[] { pBase, iP }));
}
return(pBase);
}
// fall out
return(null);
}
/// <summary>
/// Makes new parameterization for nav
/// </summary>
/// <param name="lane"></param>
/// <param name="lanePlan"></param>
/// <param name="speed"></param>
/// <param name="distance"></param>
/// <param name="stopType"></param>
/// <returns></returns>
public TravelingParameters NavStopParameterization(IFQMPlanable lane, RoadPlan roadPlan, double speed, double distance,
ArbiterWaypoint stopWaypoint, StopType stopType, VehicleState state)
{
// get min dist
double distanceCutOff = stopType == StopType.StopLine ? CoreCommon.OperationslStopLineSearchDistance : CoreCommon.OperationalStopDistance;
#region Get Decorators
// turn direction default
ArbiterTurnDirection atd = ArbiterTurnDirection.Straight;
List <BehaviorDecorator> decorators = TurnDecorators.NoDecorators;
// check if need decorators
if (lane is ArbiterLane &&
stopWaypoint.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest) &&
roadPlan.BestPlan.laneWaypointOfInterest.IsExit &&
distance < 40.0)
{
if (roadPlan.BestPlan.laneWaypointOfInterest.BestExit == null)
{
ArbiterOutput.Output("NAV BUG: lanePlan.laneWaypointOfInterest.BestExit: FQM NavStopParameterization");
}
else
{
switch (roadPlan.BestPlan.laneWaypointOfInterest.BestExit.TurnDirection)
{
case ArbiterTurnDirection.Left:
decorators = TurnDecorators.LeftTurnDecorator;
atd = ArbiterTurnDirection.Left;
break;
case ArbiterTurnDirection.Right:
atd = ArbiterTurnDirection.Right;
decorators = TurnDecorators.RightTurnDecorator;
break;
case ArbiterTurnDirection.Straight:
atd = ArbiterTurnDirection.Straight;
decorators = TurnDecorators.NoDecorators;
break;
case ArbiterTurnDirection.UTurn:
atd = ArbiterTurnDirection.UTurn;
decorators = TurnDecorators.LeftTurnDecorator;
break;
}
}
}
else if (lane is SupraLane)
{
SupraLane sl = (SupraLane)lane;
double distToInterconnect = sl.DistanceBetween(state.Front, sl.Interconnect.InitialGeneric.Position);
if ((distToInterconnect > 0 && distToInterconnect < 40.0) || sl.ClosestComponent(state.Front) == SLComponentType.Interconnect)
{
switch (sl.Interconnect.TurnDirection)
{
case ArbiterTurnDirection.Left:
decorators = TurnDecorators.LeftTurnDecorator;
atd = ArbiterTurnDirection.Left;
break;
case ArbiterTurnDirection.Right:
atd = ArbiterTurnDirection.Right;
decorators = TurnDecorators.RightTurnDecorator;
break;
case ArbiterTurnDirection.Straight:
atd = ArbiterTurnDirection.Straight;
decorators = TurnDecorators.NoDecorators;
break;
case ArbiterTurnDirection.UTurn:
atd = ArbiterTurnDirection.UTurn;
decorators = TurnDecorators.LeftTurnDecorator;
break;
}
}
}
#endregion
#region Get Maneuver
Maneuver m = new Maneuver();
bool usingSpeed = true;
SpeedCommand sc = new StopAtDistSpeedCommand(distance);
#region Distance Cutoff
// check if distance is less than cutoff
if (distance < distanceCutOff && stopType != StopType.EndOfLane)
{
// default behavior
Behavior b = new StayInLaneBehavior(stopWaypoint.Lane.LaneId, new StopAtDistSpeedCommand(distance), new List <int>(), lane.LanePath(), stopWaypoint.Lane.Width, stopWaypoint.Lane.NumberOfLanesLeft(state.Front, true), stopWaypoint.Lane.NumberOfLanesRight(state.Front, true));
// stopping so not using speed param
usingSpeed = false;
// exit is next
if (stopType == StopType.Exit)
{
// exit means stopping at a good exit in our current lane
IState nextState = new StoppingAtExitState(stopWaypoint.Lane, stopWaypoint, atd, true, roadPlan.BestPlan.laneWaypointOfInterest.BestExit, state.Timestamp, state.Front);
m = new Maneuver(b, nextState, decorators, state.Timestamp);
}
// stop line is left
else if (stopType == StopType.StopLine)
{
// determine if hte stop line is the best exit
bool isNavExit = roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Equals(stopWaypoint);
// get turn direction
atd = isNavExit ? atd : ArbiterTurnDirection.Straight;
// predetermine interconnect if best exit
ArbiterInterconnect desired = null;
if (isNavExit)
{
desired = roadPlan.BestPlan.laneWaypointOfInterest.BestExit;
}
else if (stopWaypoint.NextPartition != null && state.Front.DistanceTo(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position) > 25)
{
desired = stopWaypoint.NextPartition.ToInterconnect;
}
// set decorators
decorators = isNavExit ? decorators : TurnDecorators.NoDecorators;
// stop at the stop
IState nextState = new StoppingAtStopState(stopWaypoint.Lane, stopWaypoint, atd, isNavExit, desired);
b = new StayInLaneBehavior(stopWaypoint.Lane.LaneId, new StopAtLineSpeedCommand(), new List <int>(), lane.LanePath(), stopWaypoint.Lane.Width, stopWaypoint.Lane.NumberOfLanesLeft(state.Front, true), stopWaypoint.Lane.NumberOfLanesRight(state.Front, true));
m = new Maneuver(b, nextState, decorators, state.Timestamp);
sc = new StopAtLineSpeedCommand();
}
else if (stopType == StopType.LastGoal)
{
// stop at the last goal
IState nextState = new StayInLaneState(stopWaypoint.Lane, CoreCommon.CorePlanningState);
m = new Maneuver(b, nextState, decorators, state.Timestamp);
}
}
#endregion
#region Outisde Distance Envelope
// not inside distance envalope
else
{
// set speed
sc = new ScalarSpeedCommand(speed);
// check if lane
if (lane is ArbiterLane)
{
// get lane
ArbiterLane al = (ArbiterLane)lane;
// default behavior
Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(speed), new List <int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true));
// standard behavior is fine for maneuver
m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), decorators, state.Timestamp);
}
// check if supra lane
else if (lane is SupraLane)
{
// get lane
SupraLane sl = (SupraLane)lane;
// get sl state
StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState;
// get default beheavior
Behavior b = sisls.GetBehavior(new ScalarSpeedCommand(speed), state.Front, new List <int>());
// standard behavior is fine for maneuver
m = new Maneuver(b, sisls, decorators, state.Timestamp);
}
}
#endregion
#endregion
#region Parameterize
// create new params
TravelingParameters tp = new TravelingParameters();
tp.Behavior = m.PrimaryBehavior;
tp.Decorators = m.PrimaryBehavior.Decorators;
tp.DistanceToGo = distance;
tp.NextState = m.PrimaryState;
tp.RecommendedSpeed = speed;
tp.Type = TravellingType.Navigation;
tp.UsingSpeed = usingSpeed;
tp.SpeedCommand = sc;
tp.VehiclesToIgnore = new List <int>();
// return navigation params
return(tp);
#endregion
}
/// <summary>
/// Add blockage to interconnect
/// </summary>
/// <param name="arbiterInterconnect"></param>
public void AddInterconnectBlockage(ArbiterInterconnect arbiterInterconnect)
{
this.AddBlockage(arbiterInterconnect, CoreCommon.Communications.GetVehicleState().Position, false);
}
/// <summary>
/// Maximum speed over an interconnect
/// </summary>
/// <param name="ai"></param>
/// <returns></returns>
public static double MaximumInterconnectSpeed(ArbiterInterconnect ai)
{
// set the minimum maximum speed = 4mph
double minSpeed = 1.78816;
if (ai.InitialGeneric is ArbiterWaypoint && ai.FinalGeneric is ArbiterWaypoint)
{
// waypoint
ArbiterWaypoint awI = (ArbiterWaypoint)ai.InitialGeneric;
ArbiterWaypoint awF = (ArbiterWaypoint)ai.FinalGeneric;
List<Coordinates> interCoords = new List<Coordinates>();
Coordinates init = awI.Position - awI.PreviousPartition.Vector().Normalize(10.0);
Coordinates fin = awF.Position + awF.NextPartition.Vector().Normalize(10.0);
interCoords.Add(init);
interCoords.Add(awI.Position);
interCoords.Add(awF.Position);
interCoords.Add(fin);
double initMax = awI.Lane.Way.Segment.SpeedLimits.MaximumSpeed;
double finalMax = awF.Lane.Way.Segment.SpeedLimits.MaximumSpeed;
double curvatureMax = MaximumSpeed(interCoords, minSpeed);
return Math.Min(Math.Min(initMax, finalMax), curvatureMax);
}
else
{
return minSpeed;
}
}
/// <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
}
