/// <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="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>
/// Waypoints to ignore
/// </summary>
/// <param name="way"></param>
/// <param name="position"></param>
/// <returns></returns>
public static List<ArbiterWaypoint> WaypointsClose(ArbiterWay way, Coordinates position, ArbiterWaypoint ignoreSpecifically)
{
List<ArbiterWaypoint> waypoints = new List<ArbiterWaypoint>();
foreach (ArbiterLane al in way.Lanes.Values)
{
ArbiterWaypoint aw = al.GetClosestWaypoint(position, TahoeParams.VL * 2.0);
if(aw != null)
waypoints.Add(aw);
}
if (ignoreSpecifically != null && !waypoints.Contains(ignoreSpecifically))
waypoints.Add(ignoreSpecifically);
return waypoints;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="globalMonitor"></param>
/// <param name="involved"></param>
public DominantLaneEntryMonitor(IntersectionMonitor globalMonitor, IntersectionInvolved involved)
{
this.waitingTimer = new Stopwatch();
this.globalMonitor = globalMonitor;
this.lane = (ArbiterLane)involved.Area;
this.involved = involved;
if (involved.Exit != null)
this.exit = (ArbiterWaypoint)involved.Exit;
else
this.exit = null;
if (this.exit != null)
{
// create the polygon
this.exitPolygon = this.ExitPolygon();
}
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="globalMonitor"></param>
/// <param name="stop"></param>
public SubmissiveEntryMonitor(IntersectionMonitor globalMonitor, ArbiterWaypoint stop, bool isOurs)
{
this.globalMonitor = globalMonitor;
this.timer = new Stopwatch();
this.isOurs = isOurs;
foreach (ArbiterStoppedExit ase in this.globalMonitor.Intersection.StoppedExits)
{
if (ase.Waypoint.Equals(stop))
{
this.stoppedExit = ase;
}
}
if (this.stoppedExit == null)
throw new Exception("needs stopped exit");
}
/// <summary>
/// Turn information
/// </summary>
/// <param name="entry"></param>
/// <param name="finalPath"></param>
/// <param name="leftBound"></param>
/// <param name="rightBound"></param>
public static void TurnInfo(ArbiterWaypoint entry, out LinePath finalPath, out LineList leftBound, out LineList rightBound)
{
if (entry.NextPartition != null)
{
double distance = entry.NextPartition.Length;
// get left bound
rightBound = GeneralToolkit.TranslateVector(entry.Position, entry.NextPartition.Final.Position,
entry.NextPartition.Vector().Normalize(entry.Lane.Width / 2.0).RotateM90());
// get right bound
leftBound = GeneralToolkit.TranslateVector(entry.Position, entry.NextPartition.Final.Position,
entry.NextPartition.Vector().Normalize(entry.Lane.Width / 2.0).Rotate90());
ArbiterWaypoint current = entry.NextPartition.Final;
while (current.NextPartition != null && distance < 50)
{
distance += current.NextPartition.Length;
LineList rtTemp = GeneralToolkit.TranslateVector(current.Position, current.NextPartition.Final.Position,
current.NextPartition.Vector().Normalize(current.Lane.Width / 2.0).RotateM90());
rightBound.Add(rtTemp[rtTemp.Count - 1]);
LineList ltTemp = GeneralToolkit.TranslateVector(current.Position, current.NextPartition.Final.Position,
current.NextPartition.Vector().Normalize(current.Lane.Width / 2.0).Rotate90());
leftBound.Add(ltTemp[ltTemp.Count - 1]);
current = current.NextPartition.Final;
}
finalPath = entry.Lane.LanePath(entry, 50.0);
}
else
{
Coordinates final = entry.Position + entry.PreviousPartition.Vector().Normalize(TahoeParams.VL);
finalPath = new LinePath(new Coordinates[] { entry.Position, final });
LinePath lB = finalPath.ShiftLateral(entry.Lane.Width / 2.0);
LinePath rB = finalPath.ShiftLateral(-entry.Lane.Width / 2.0);
leftBound = new LineList(lB);
rightBound = new LineList(rB);
}
}
/// <summary>
/// Distance between two waypoints
/// </summary>
/// <param name="w1"></param>
/// <param name="w2"></param>
/// <returns></returns>
public double DistanceBetween(ArbiterWaypoint w1, ArbiterWaypoint w2)
{
return(this.LanePath(w1, w2).PathLength);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="aw"></param>
/// <param name="ep"></param>
public ArbiterStoppedExit(ArbiterWaypoint aw, Polygon ep)
{
this.Waypoint = aw;
this.ExitPolygon = ep;
}
/// <summary>
/// Get the next waypoint of a certain type
/// </summary>
/// <param name="w1">Starting waypoint of search</param>
/// <param name="wt">Waypoint type to look for</param>
/// <returns></returns>
public ArbiterWaypoint GetNext(ArbiterWaypoint w1, WaypointType wt)
{
return(this.GetNext(w1, wt, new List <ArbiterWaypoint>()));
}
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>
/// Constructor
/// </summary>
/// <param name="aw"></param>
public IgnorableWaypoint(ArbiterWaypoint aw)
{
this.Waypoint = aw;
this.numberOfCyclesIgnored = 0;
}
/// <summary>
/// Gets next waypoint of a certain type ignoring certain waypoints
/// </summary>
/// <param name="w1"></param>
/// <param name="wt"></param>
/// <param name="ignorable"></param>
/// <returns></returns>
public ArbiterWaypoint GetNext(ArbiterWaypoint w1, WaypointType wt, List<ArbiterWaypoint> ignorable)
{
ArbiterWaypoint tmp = w1;
while (tmp != null)
{
if (tmp.WaypointTypeEquals(wt) && !ignorable.Contains(tmp))
return tmp;
if (tmp.NextPartition != null)
tmp = tmp.NextPartition.Final;
else
tmp = null;
}
return null;
}
/// <summary>
/// Path of lane from a waypoint for a certain distance
/// </summary>
/// <param name="w1">Initial waypoint</param>
/// <param name="distance">Distance to get path</param>
/// <returns></returns>
public LinePath LanePath(ArbiterWaypoint w1, double distance)
{
return this.LanePath().SubPath(this.LanePath().GetClosestPoint(w1.Position), distance);
}
/// <summary>
/// Adds waypoint to ignore list
/// </summary>
/// <param name="toIgnore"></param>
public void IgnoreWaypoint(ArbiterWaypoint toIgnore)
{
this.IgnorableWaypoints.Add(new IgnorableWaypoint(toIgnore));
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="turnFinal"></param>
public LaneEntryAreaMonitor(ArbiterWaypoint turnFinal)
{
this.turnFinalWaypoint = turnFinal;
this.timer = new Stopwatch();
}
/// <summary>
/// Gets time cost between two waypoints in the same lane
/// </summary>
/// <param name="initial"></param>
/// <param name="final"></param>
/// <returns></returns>
private double TimeCostInLane(ArbiterWaypoint initial, ArbiterWaypoint final)
{
ArbiterWaypoint current = initial;
double cost = 0.0;
while (current != null)
{
if (current.Equals(final))
{
return cost;
}
else
{
if (current.NextPartition == null)
return cost;
cost += current.NextPartition.TimeCost();
if (current.NextPartition != null)
current = current.NextPartition.Final;
else
current = null;
}
}
return Double.MaxValue;
}
/// <summary>
/// Path of lane from a waypoint for a certain distance
/// </summary>
/// <param name="w1">Initial waypoint</param>
/// <param name="distance">Distance to get path</param>
/// <returns></returns>
public LinePath LanePath(ArbiterWaypoint w1, double distance)
{
return(this.LanePath().SubPath(this.LanePath().GetClosestPoint(w1.Position), distance));
}
/// <summary>
/// Gets next entry downstream from this waypoint
/// </summary>
/// <param name="waypoint"></param>
/// <returns></returns>
private ArbiterWaypoint NextEntry(ArbiterWaypoint waypoint)
{
if (waypoint.NextPartition != null)
{
List<DownstreamPointOfInterest> entriesDownstream = this.Downstream(waypoint.Position, waypoint.Lane.Way, false, new List<ArbiterWaypoint>());
return entriesDownstream.Count > 0 ? entriesDownstream[0].PointOfInterest : null;
}
else
{
return null;
}
}
/// <summary>
/// Generates the xySegments into segments and inputs them into the input road network
/// </summary>
/// <param name="arn"></param>
/// <returns></returns>
public ArbiterRoadNetwork GenerateSegments(ArbiterRoadNetwork arn)
{
foreach (SimpleSegment ss in segments)
{
// seg
ArbiterSegmentId asi = new ArbiterSegmentId(int.Parse(ss.Id));
ArbiterSegment asg = new ArbiterSegment(asi);
arn.ArbiterSegments.Add(asi, asg);
asg.RoadNetwork = arn;
asg.SpeedLimits = new ArbiterSpeedLimit();
asg.SpeedLimits.MaximumSpeed = 13.4112; // 30mph max speed
// way1
ArbiterWayId awi1 = new ArbiterWayId(1, asi);
ArbiterWay aw1 = new ArbiterWay(awi1);
aw1.Segment = asg;
asg.Ways.Add(awi1, aw1);
asg.Way1 = aw1;
// way2
ArbiterWayId awi2 = new ArbiterWayId(2, asi);
ArbiterWay aw2 = new ArbiterWay(awi2);
aw2.Segment = asg;
asg.Ways.Add(awi2, aw2);
asg.Way2 = aw2;
// make lanes
foreach (SimpleLane sl in ss.Lanes)
{
// lane
ArbiterLaneId ali;
ArbiterLane al;
// get way of lane id
if (ss.Way1Lanes.Contains(sl))
{
ali = new ArbiterLaneId(GenerationTools.GetId(sl.Id)[1], awi1);
al = new ArbiterLane(ali);
aw1.Lanes.Add(ali, al);
al.Way = aw1;
}
else
{
ali = new ArbiterLaneId(GenerationTools.GetId(sl.Id)[1], awi2);
al = new ArbiterLane(ali);
aw2.Lanes.Add(ali, al);
al.Way = aw2;
}
// add to display
arn.DisplayObjects.Add(al);
// width
al.Width = sl.LaneWidth == 0 ? TahoeParams.T * 2.0 : sl.LaneWidth * 0.3048;
if (sl.LaneWidth == 0)
{
Console.WriteLine("lane: " + ali.ToString() + " contains no lane width, setting to 4m");
}
// lane boundaries
al.BoundaryLeft = this.GenerateLaneBoundary(sl.LeftBound);
al.BoundaryRight = this.GenerateLaneBoundary(sl.RightBound);
// add lane to seg
asg.Lanes.Add(ali, al);
// waypoints
List <ArbiterWaypoint> waypointList = new List <ArbiterWaypoint>();
// generate waypoints
foreach (SimpleWaypoint sw in sl.Waypoints)
{
// waypoint
ArbiterWaypointId awi = new ArbiterWaypointId(GenerationTools.GetId(sw.ID)[2], ali);
ArbiterWaypoint aw = new ArbiterWaypoint(sw.Position, awi);
aw.Lane = al;
// stop
if (sl.Stops.Contains(sw.ID))
{
aw.IsStop = true;
}
// checkpoint
foreach (SimpleCheckpoint sc in sl.Checkpoints)
{
if (sw.ID == sc.WaypointId)
{
aw.IsCheckpoint = true;
aw.CheckpointId = int.Parse(sc.CheckpointId);
arn.Checkpoints.Add(aw.CheckpointId, aw);
}
}
// add
asg.Waypoints.Add(awi, aw);
arn.ArbiterWaypoints.Add(awi, aw);
al.Waypoints.Add(awi, aw);
waypointList.Add(aw);
arn.DisplayObjects.Add(aw);
arn.LegacyWaypointLookup.Add(sw.ID, aw);
}
al.WaypointList = waypointList;
// lane partitions
List <ArbiterLanePartition> alps = new List <ArbiterLanePartition>();
al.Partitions = alps;
// generate lane partitions
for (int i = 0; i < waypointList.Count - 1; i++)
{
// create lane partition
ArbiterLanePartitionId alpi = new ArbiterLanePartitionId(waypointList[i].WaypointId, waypointList[i + 1].WaypointId, ali);
ArbiterLanePartition alp = new ArbiterLanePartition(alpi, waypointList[i], waypointList[i + 1], asg);
alp.Lane = al;
waypointList[i].NextPartition = alp;
waypointList[i + 1].PreviousPartition = alp;
alps.Add(alp);
arn.DisplayObjects.Add(alp);
// crete initial user partition
ArbiterUserPartitionId aupi = new ArbiterUserPartitionId(alp.PartitionId, waypointList[i].WaypointId, waypointList[i + 1].WaypointId);
ArbiterUserPartition aup = new ArbiterUserPartition(aupi, alp, waypointList[i], waypointList[i + 1]);
List <ArbiterUserPartition> aups = new List <ArbiterUserPartition>();
aups.Add(aup);
alp.UserPartitions = aups;
alp.SetDefaultSparsePolygon();
arn.DisplayObjects.Add(aup);
}
// path segments of lane
List <IPathSegment> ips = new List <IPathSegment>();
List <Coordinates> pathSegments = new List <Coordinates>();
pathSegments.Add(alps[0].Initial.Position);
// loop
foreach (ArbiterLanePartition alPar in alps)
{
ips.Add(new LinePathSegment(alPar.Initial.Position, alPar.Final.Position));
// make new segment
pathSegments.Add(alPar.Final.Position);
}
// generate lane partition path
LinePath partitionPath = new LinePath(pathSegments);
al.SetLanePath(partitionPath);
al.PartitionPath = new Path(ips, CoordinateMode.AbsoluteProjected);
// safeto zones
foreach (ArbiterWaypoint aw in al.Waypoints.Values)
{
if (aw.IsStop)
{
LinePath.PointOnPath end = al.GetClosestPoint(aw.Position);
double dist = -30;
LinePath.PointOnPath begin = al.LanePath().AdvancePoint(end, ref dist);
if (dist != 0)
{
begin = al.LanePath().StartPoint;
}
ArbiterSafetyZone asz = new ArbiterSafetyZone(al, end, begin);
asz.isExit = true;
asz.Exit = aw;
al.SafetyZones.Add(asz);
arn.DisplayObjects.Add(asz);
arn.ArbiterSafetyZones.Add(asz);
}
}
}
}
return(arn);
}
/// <summary>
/// Updates the montitor with the closest forward vehicle along the lane
/// </summary>
/// <param name="vehicleState"></param>
/// <param name="final"></param>
public void Update(VehicleState vehicleState, ArbiterWaypoint final, LinePath fullPath)
{
this.VehiclesToIgnore = new List<int>();
if (TacticalDirector.VehicleAreas.ContainsKey(final.Lane))
{
List<VehicleAgent> laneVehicles = TacticalDirector.VehicleAreas[final.Lane];
this.CurrentVehicle = null;
double currentDistance = Double.MaxValue;
foreach (VehicleAgent va in laneVehicles)
{
double endDistance = final.Lane.DistanceBetween(final.Position, va.ClosestPosition);
if (endDistance > 0)
this.VehiclesToIgnore.Add(va.VehicleId);
double tmpDist = endDistance;
if (tmpDist > 0 && (this.CurrentVehicle == null || tmpDist < currentDistance))
{
this.CurrentVehicle = va;
currentDistance = tmpDist;
}
}
}
else
this.CurrentVehicle = null;
}
/// <summary>
/// Distance between two waypoints
/// </summary>
/// <param name="w1"></param>
/// <param name="w2"></param>
/// <returns></returns>
public double DistanceBetween(ArbiterWaypoint w1, ArbiterWaypoint w2)
{
return this.LanePath(w1, w2).PathLength;
}
/// <summary>
/// Checks if intersection contains entry ownstream in lane from waypoint
/// </summary>
/// <param name="aw"></param>
/// <returns></returns>
public ArbiterWaypoint EntryDownstream(ArbiterWaypoint aw)
{
foreach (ITraversableWaypoint itw in this.Intersection.AllEntries.Values)
{
if (itw is ArbiterWaypoint)
{
ArbiterWaypoint testWaypoint = (ArbiterWaypoint)itw;
if (aw.Lane.Equals(testWaypoint.Lane) && aw.WaypointId.Number < testWaypoint.WaypointId.Number)
{
return testWaypoint;
}
}
}
return null;
}
/// <summary>
/// Get the next waypoint of a certain type
/// </summary>
/// <param name="w1">Starting waypoint of search</param>
/// <param name="wt">Waypoint type to look for</param>
/// <returns></returns>
public ArbiterWaypoint GetNext(ArbiterWaypoint w1, WaypointType wt)
{
ArbiterWaypoint tmp = w1;
while (tmp != null)
{
if (tmp.WaypointTypeEquals(wt))
return tmp;
if (tmp.NextPartition != null)
tmp = tmp.NextPartition.Final;
else
tmp = null;
}
return null;
}
/// <summary>
/// Determines proper speed commands given we want to stop at a certain waypoint
/// </summary>
/// <param name="waypoint"></param>
/// <param name="lane"></param>
/// <param name="position"></param>
/// <param name="enCovariance"></param>
/// <param name="stopSpeed"></param>
/// <param name="stopDistance"></param>
public void StoppingParams(ArbiterWaypoint waypoint, ArbiterLane lane, Coordinates position, double extraDistance,
out double stopSpeed, out double stopDistance)
{
// get dist to waypoint
stopDistance = lane.DistanceBetween(position, waypoint.Position) - extraDistance;
// speed tools
stopSpeed = SpeedTools.GenerateSpeed(stopDistance, CoreCommon.OperationalStopSpeed, 2.24);
}
/// <summary>
/// Gets next waypoint of a certain type ignoring certain waypoints
/// </summary>
/// <param name="w1"></param>
/// <param name="wt"></param>
/// <param name="ignorable"></param>
/// <returns></returns>
public ArbiterWaypoint GetNext(ArbiterWaypoint w1, List<WaypointType> wts, List<ArbiterWaypoint> ignorable)
{
ArbiterWaypoint tmp = w1;
while (tmp != null)
{
bool back = false;
foreach(WaypointType wt in wts)
{
if(tmp.WaypointTypeEquals(wt))
back = true;
}
if (back && !ignorable.Contains(tmp))
return tmp;
if (tmp.NextPartition != null)
tmp = tmp.NextPartition.Final;
else
tmp = null;
}
return null;
}
/// <summary>
/// Generate the traveling parameterization for the desired behaivor
/// </summary>
/// <param name="lane"></param>
/// <param name="navStopSpeed"></param>
/// <param name="navStopDistance"></param>
/// <param name="navStop"></param>
/// <param name="navStopType"></param>
/// <param name="state"></param>
/// <returns></returns>
private TravelingParameters NavStopParameterization(ArbiterLane lane, double navStopSpeed, double navStopDistance, ArbiterWaypoint navStop, StopType navStopType, VehicleState state)
{
// get min dist
double distanceCutOff = CoreCommon.OperationalStopDistance;
// turn direction default
List<BehaviorDecorator> decorators = TurnDecorators.NoDecorators;
// create new params
TravelingParameters tp = new TravelingParameters();
#region Get Maneuver
Maneuver m = new Maneuver();
bool usingSpeed = true;
// get lane path
LinePath lp = lane.LanePath().Clone();
lp.Reverse();
#region Distance Cutoff
// check if distance is less than cutoff
if (navStopDistance < distanceCutOff)
{
// default behavior
tp.SpeedCommand = new StopAtDistSpeedCommand(navStopDistance);
Behavior b = new StayInLaneBehavior(lane.LaneId, new StopAtDistSpeedCommand(navStopDistance), new List<int>(), lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false));
// stopping so not using speed param
usingSpeed = false;
IState nextState = CoreCommon.CorePlanningState;
m = new Maneuver(b, nextState, decorators, state.Timestamp);
}
#endregion
#region Outisde Distance Envelope
// not inside distance envalope
else
{
// get lane
ArbiterLane al = lane;
// default behavior
tp.SpeedCommand = new ScalarSpeedCommand(Math.Min(navStopSpeed, 2.24));
Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(Math.Min(navStopSpeed, 2.24)), new List<int>(), lp, al.Width, al.NumberOfLanesRight(state.Front, false), al.NumberOfLanesLeft(state.Front, false));
// standard behavior is fine for maneuver
m = new Maneuver(b, CoreCommon.CorePlanningState, decorators, state.Timestamp);
}
#endregion
#endregion
#region Parameterize
tp.Behavior = m.PrimaryBehavior;
tp.Decorators = m.PrimaryBehavior.Decorators;
tp.DistanceToGo = navStopDistance;
tp.NextState = m.PrimaryState;
tp.RecommendedSpeed = navStopSpeed;
tp.Type = TravellingType.Navigation;
tp.UsingSpeed = usingSpeed;
tp.VehiclesToIgnore = new List<int>();
// return navigation params
return tp;
#endregion
}
/// <summary>
/// Path of lane between two waypoints
/// </summary>
/// <param name="w1">Initial waypoint</param>
/// <param name="w2">Final waypoint</param>
/// <returns></returns>
public LinePath LanePath(ArbiterWaypoint w1, ArbiterWaypoint w2)
{
return this.LanePath().SubPath(
this.LanePath().GetClosestPoint(w1.Position),
this.LanePath().GetClosestPoint(w2.Position));
}
/// <summary>
/// Generates entry adjacency
/// </summary>
/// <param name="arn"></param>
/// <remarks>Determines for every entry in a segment, the closest, reachable
/// waypoints on lanes in the same way if it exists</remarks>
private void GenerateNavigationalAdjacency(ArbiterRoadNetwork arn)
{
// loop over segments
foreach (ArbiterSegment asg in arn.ArbiterSegments.Values)
{
// loop over segment waypoints
foreach (ArbiterWaypoint aw in asg.Waypoints.Values)
{
#region Next Waypoint
// check if has next
if (aw.NextPartition != null)
{
// add next waypoint
aw.OutgoingConnections.Add(aw.NextPartition);
}
#endregion
#region Exits
// check if exit
if (aw.IsExit)
{
// loop over interconnect
foreach (ArbiterInterconnect ai in aw.Exits)
{
// add entries
aw.OutgoingConnections.Add(ai);
}
}
#endregion
#region Adjacent Lanes
// check if entry
if (aw.IsEntry)
{
// foreach lane test in same way as aw
foreach (ArbiterLane al in aw.Lane.Way.Lanes.Values)
{
// check if not same lane
if (!aw.Lane.Equals(al) && al.RelativelyInside(aw.Position))
{
// check ok
if ((aw.Lane.LaneOnLeft != null && aw.Lane.LaneOnLeft.Equals(al) && aw.Lane.BoundaryLeft != ArbiterLaneBoundary.SolidWhite) ||
(aw.Lane.LaneOnRight != null && aw.Lane.LaneOnRight.Equals(al) && aw.Lane.BoundaryRight != ArbiterLaneBoundary.SolidWhite))
{
// get closest partition to aw's point
ArbiterLanePartition alp = al.GetClosestPartition(aw.Position);
// check downstream from this lane?
if (aw.Lane.DistanceBetween(aw.Position, alp.Final.Position) >= -0.01 ||
(alp.Final.NextPartition != null && aw.Lane.DistanceBetween(aw.Position, alp.Final.NextPartition.Final.Position) >= -0.01))
{
// new list of contained partitions
List <IConnectAreaWaypoints> containedPartitions = new List <IConnectAreaWaypoints>();
containedPartitions.Add(alp);
containedPartitions.Add(aw.NextPartition);
// set aw as linking to that partition's final waypoint
aw.OutgoingConnections.Add(new NavigableEdge(false, null, true, al.Way.Segment, containedPartitions, aw, alp.Final));
}
}
}
}
}
#endregion
}
#region Adjacent starting in middle
// loop over segment lanes
foreach (ArbiterLane al in asg.Lanes.Values)
{
// get init wp
ArbiterWaypoint initial = al.WaypointList[0];
// get adjacent lanes
foreach (ArbiterLane adj in al.Way.Lanes.Values)
{
if (!adj.Equals(al))
{
// check if initial is inside other
if (adj.RelativelyInside(initial.Position))
{
if ((adj.LaneOnLeft != null && adj.LaneOnLeft.Equals(al) && adj.BoundaryLeft != ArbiterLaneBoundary.SolidWhite) ||
(adj.LaneOnRight != null && adj.LaneOnRight.Equals(al) && adj.BoundaryRight != ArbiterLaneBoundary.SolidWhite))
{
// closest partition
ArbiterLanePartition alp = adj.GetClosestPartition(initial.Position);
// new list of contained partitions
List <IConnectAreaWaypoints> containedPartitions = new List <IConnectAreaWaypoints>();
containedPartitions.Add(alp);
containedPartitions.Add(initial.NextPartition);
// set aw as linking to that partition's final waypoint
alp.Initial.OutgoingConnections.Add(new NavigableEdge(false, null, true, al.Way.Segment, containedPartitions, alp.Initial, initial));
}
}
}
}
}
#endregion
}
// loop over zones
foreach (ArbiterZone az in arn.ArbiterZones.Values)
{
#region Perimeter Point adjacency
// loop over zone perimeter points
foreach (ArbiterPerimeterWaypoint apw in az.Perimeter.PerimeterPoints.Values)
{
#region Old Connectivity
// check if this is an entry
/*if (apw.IsEntry)
* {
#region Link Perimeter Points
*
* // loop over perimeter points
* foreach(ArbiterPerimeterWaypoint apwTest in az.Perimeter.PerimeterPoints.Values)
* {
* // check not this and is exit
* if (!apw.Equals(apwTest) && apwTest.IsExit)
* {
* // add to connections
* apw.OutgoingConnections.Add(new NavigableEdge(true, apw.Perimeter.Zone, false, null, new List<IConnectAreaWaypoints>(), apw, apwTest));
* }
* }
*
#endregion
*
#region Link Checkpoints
*
* // loop over parking spot waypoints
* foreach (ArbiterParkingSpotWaypoint apsw in az.ParkingSpotWaypoints.Values)
* {
* // check if checkpoint
* if (apsw.IsCheckpoint)
* {
* // add to connections
* apw.OutgoingConnections.Add(new NavigableEdge(true, apw.Perimeter.Zone, false, null, new List<IConnectAreaWaypoints>(), apw, apsw));
* }
* }
*
#endregion
* }*/
#endregion
// check if the point is an exit
if (apw.IsExit)
{
foreach (ArbiterInterconnect ai in apw.Exits)
{
// add to connections
apw.OutgoingConnections.Add(ai);
}
}
}
#endregion
#region Checkpoint adjacency
// loop over parking spot waypoints
foreach (ArbiterParkingSpotWaypoint apsw in az.ParkingSpotWaypoints.Values)
{
if (apsw.ParkingSpot.NormalWaypoint.Equals(apsw))
{
apsw.OutgoingConnections.Add(
new NavigableEdge(true, az, false, null, new List <IConnectAreaWaypoints>(), apsw, apsw.ParkingSpot.Checkpoint));
}
else
{
apsw.OutgoingConnections.Add(
new NavigableEdge(true, az, false, null, new List <IConnectAreaWaypoints>(), apsw, apsw.ParkingSpot.NormalWaypoint));
}
}
#region Old
/*
* // loop over parking spot waypoints
* foreach (ArbiterParkingSpotWaypoint apsw in az.ParkingSpotWaypoints.Values)
* {
* // check if checkpoint
* if (apsw.IsCheckpoint)
* {
#region Link Perimeter Points
*
* // loop over perimeter points
* foreach (ArbiterPerimeterWaypoint apwTest in az.Perimeter.PerimeterPoints.Values)
* {
* // check not this and is exit
* if (apwTest.IsExit)
* {
* // add to connections
* apsw.OutgoingConnections.Add(new NavigableEdge(true, apsw.ParkingSpot.Zone, false, null, new List<IConnectAreaWaypoints>(), apsw, apwTest));
* }
* }
*
#endregion
*
#region Link Checkpoints
*
* // loop over parking spot waypoints
* foreach (ArbiterParkingSpotWaypoint apswTest in az.ParkingSpotWaypoints.Values)
* {
* // check if checkpoint
* if (!apsw.Equals(apswTest) && apswTest.IsCheckpoint)
* {
* // add to connections
* apsw.OutgoingConnections.Add(new NavigableEdge(true, apsw.ParkingSpot.Zone, false, null, new List<IConnectAreaWaypoints>(), apsw, apswTest));
* }
* }
*
#endregion
* }
* }
*/
#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>
/// Next point at which to stop
/// </summary>
/// <param name="lane"></param>
/// <param name="position"></param>
/// <param name="stopPoint"></param>
/// <param name="stopSpeed"></param>
/// <param name="stopDistance"></param>
public void NextLaneStop(IFQMPlanable lane, Coordinates position, double[] enCovariance, List<ArbiterWaypoint> ignorable,
out ArbiterWaypoint stopPoint, out double stopSpeed, out double stopDistance, out StopType stopType)
{
// get the stop
List<WaypointType> wts = new List<WaypointType>();
wts.Add(WaypointType.Stop);
wts.Add(WaypointType.End);
stopPoint = lane.GetNext(position, wts, ignorable);
// set stop type
stopType = stopPoint.IsStop ? StopType.StopLine : StopType.EndOfLane;
// parameterize
this.StoppingParams(stopPoint, lane, position, enCovariance, out stopSpeed, out stopDistance);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="aw"></param>
/// <param name="cycles"></param>
public IgnorableWaypoint(ArbiterWaypoint aw, int cycles)
{
this.Waypoint = aw;
this.numberOfCyclesIgnored = cycles;
}
/// <summary>
/// Determines proper speed commands given we want to stop at a certain waypoint
/// </summary>
/// <param name="waypoint"></param>
/// <param name="lane"></param>
/// <param name="position"></param>
/// <param name="enCovariance"></param>
/// <param name="stopSpeed"></param>
/// <param name="stopDistance"></param>
public void StoppingParams(ArbiterWaypoint waypoint, IFQMPlanable lane, Coordinates position, double[] enCovariance,
out double stopSpeed, out double stopDistance)
{
// get dist to waypoint
stopDistance = lane.DistanceBetween(position, waypoint.Position);
// subtract distance based upon type to help calculate speed
double stopTypeDistance = waypoint.IsStop ? CoreCommon.OperationslStopLineSearchDistance : CoreCommon.OperationalStopDistance;
double stopSpeedDistance = stopDistance - stopTypeDistance;
// check if we are positive distance away
if (stopSpeedDistance >= 0)
{
// segment max speed
double segmentMaxSpeed = lane.CurrentMaximumSpeed(position);
// get speed using constant acceleration
stopSpeed = SpeedTools.GenerateSpeed(stopSpeedDistance, CoreCommon.OperationalStopSpeed, segmentMaxSpeed);
}
else
{
// inside stop dist
stopSpeed = (stopDistance / stopTypeDistance) * CoreCommon.OperationalStopSpeed;
stopSpeed = stopSpeed < 0 ? 0.0 : stopSpeed;
}
}
/// <summary>
/// Gets the next navigational stop relavant to us (stop or end) in the closest good lane or our current opposing lane
/// </summary>
/// <param name="closestGood"></param>
/// <param name="coordinates"></param>
/// <param name="ignorable"></param>
/// <param name="navStopSpeed"></param>
/// <param name="navStopDistance"></param>
/// <param name="navStopType"></param>
/// <param name="navStop"></param>
private void NextOpposingNavigationalStop(ArbiterLane opposing, ArbiterLane closestGood, Coordinates coordinates, double extraDistance,
out double navStopSpeed, out double navStopDistance, out StopType navStopType, out ArbiterWaypoint navStop)
{
ArbiterWaypoint current = null;
double minDist = Double.MaxValue;
StopType st = StopType.EndOfLane;
#region Closest Good Parameterization
foreach (ArbiterWaypoint aw in closestGood.WaypointList)
{
if (aw.IsStop || aw.NextPartition == null)
{
double dist = closestGood.DistanceBetween(coordinates, aw.Position);
if (dist < minDist && dist >= 0)
{
current = aw;
minDist = dist;
st = aw.IsStop ? StopType.StopLine : StopType.EndOfLane;
}
}
}
#endregion
#region Opposing Parameterization
ArbiterWaypoint opStart = opposing.GetClosestPartition(coordinates).Initial;
int startIndex = opposing.WaypointList.IndexOf(opStart);
for (int i = startIndex; i >= 0; i--)
{
ArbiterWaypoint aw = opposing.WaypointList[i];
if (aw.IsStop || aw.PreviousPartition == null)
{
double dist = opposing.DistanceBetween(aw.Position, coordinates);
if (dist < minDist && dist >= 0)
{
current = aw;
minDist = dist;
st = aw.IsStop ? StopType.StopLine : StopType.EndOfLane;
}
}
}
#endregion
double tmpDistanceIgnore;
this.StoppingParams(current, closestGood, coordinates, extraDistance, out navStopSpeed, out tmpDistanceIgnore);
navStop = current;
navStopDistance = minDist;
navStopType = st;
}
/// <summary>
/// Path of lane between two waypoints
/// </summary>
/// <param name="w1">Initial waypoint</param>
/// <param name="w2">Final waypoint</param>
/// <returns></returns>
public LinePath LanePath(ArbiterWaypoint w1, ArbiterWaypoint w2)
{
return(this.LanePath().SubPath(
this.LanePath().GetClosestPoint(w1.Position),
this.LanePath().GetClosestPoint(w2.Position)));
}
/// <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>
/// Check if found a stop in a lane as part of exits or a test waypoint
/// </summary>
/// <param name="initialTest"></param>
/// <param name="exits"></param>
/// <param name="lane"></param>
/// <returns></returns>
private bool FoundStop(ArbiterWaypoint initialTest, IEnumerable<ITraversableWaypoint> exits, ArbiterLane lane)
{
if (initialTest != null && initialTest.IsStop)
return true;
foreach (ITraversableWaypoint exit in exits)
{
if (exit is ArbiterWaypoint)
{
ArbiterWaypoint aw = (ArbiterWaypoint)exit;
if (aw.IsStop && aw.Lane.Equals(lane))
{
return true;
}
}
}
return false;
}
/// <summary>
/// Determines the point at which we need to stop in the current lane
/// </summary>
/// <param name="lanePoint"></param>
/// <param name="position"></param>
/// <param name="stopRequired"></param>
/// <param name="stopSpeed"></param>
/// <param name="stopDistance"></param>
public void NextNavigationalStop(IFQMPlanable lane, ArbiterWaypoint lanePoint, Coordinates position, double[] enCovariance, List<ArbiterWaypoint> ignorable,
out double stopSpeed, out double stopDistance, out StopType stopType, out ArbiterWaypoint stopWaypoint)
{
// variables for default next stop line or end of lane
this.NextLaneStop(lane, position, enCovariance, ignorable, out stopWaypoint, out stopSpeed, out stopDistance, out stopType);
if (lanePoint != null)
{
// check if the point downstream is the last checkpoint
if (CoreCommon.Mission.MissionCheckpoints.Count == 1 && lanePoint.WaypointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId))
{
ArbiterWaypoint cpStop = lanePoint;
double cpStopSpeed;
double cpStopDistance;
StopType cpStopType = StopType.LastGoal;
this.StoppingParams(cpStop, lane, position, enCovariance, out cpStopSpeed, out cpStopDistance);
if (cpStopDistance <= stopDistance)
{
stopSpeed = cpStopSpeed;
stopDistance = cpStopDistance;
stopType = cpStopType;
stopWaypoint = cpStop;
}
}
// check if point is not the checkpoint and is an exit
else if (lanePoint.IsExit && !lanePoint.WaypointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId))
{
ArbiterWaypoint exitStop = lanePoint;
double exitStopSpeed;
double exitStopDistance;
StopType exitStopType = lanePoint.IsStop ? StopType.StopLine : StopType.Exit;
this.StoppingParams(exitStop, lane, position, enCovariance, out exitStopSpeed, out exitStopDistance);
if (exitStopDistance <= stopDistance)
{
stopSpeed = exitStopSpeed;
stopDistance = exitStopDistance;
stopType = exitStopType;
stopWaypoint = exitStop;
}
}
}
}
/// <summary>
/// Gets exits downstream, or the goal we are currently reaching
/// </summary>
/// <param name="currentPosition"></param>
/// <param name="ignorable"></param>
/// <param name="goal"></param>
/// <returns></returns>
public List <DownstreamPointOfInterest> Downstream(Coordinates currentPosition, List <ArbiterWaypoint> ignorable, INavigableNode goal)
{
// downstream final
List <DownstreamPointOfInterest> waypoints = new List <DownstreamPointOfInterest>();
foreach (ArbiterLane al in Way.Lanes.Values)
{
if (al.Equals(this) || (al.GetClosestPartition(currentPosition).Type != PartitionType.Startup &&
((this.LaneOnLeft != null && this.LaneOnLeft.Equals(al) && this.BoundaryLeft != ArbiterLaneBoundary.SolidWhite) ||
(this.LaneOnRight != null && this.LaneOnRight.Equals(al) && this.BoundaryRight != ArbiterLaneBoundary.SolidWhite))))
{
// get starting waypoint
ArbiterWaypoint waypoint = null;
// get closest partition
ArbiterLanePartition alp = al.GetClosestPartition(currentPosition);
if (alp.Initial.Position.DistanceTo(currentPosition) < TahoeParams.VL - 2)
{
waypoint = alp.Initial;
}
else if (alp.IsInside(currentPosition) || alp.Final.Position.DistanceTo(currentPosition) < TahoeParams.VL)
{
waypoint = alp.Final;
}
else if (alp.Initial.Position.DistanceTo(currentPosition) < alp.Final.Position.DistanceTo(currentPosition))
{
waypoint = alp.Initial;
}
else if (alp.Initial.Position.DistanceTo(currentPosition) < alp.Final.Position.DistanceTo(currentPosition) && alp.Final.NextPartition == null)
{
waypoint = null;
}
else
{
waypoint = null;
}
// check waypoint exists
if (waypoint != null)
{
// save start
ArbiterWaypoint initial = waypoint;
// initial cost
double initialCost = 0.0;
if (al.Equals(this))
{
initialCost = currentPosition.DistanceTo(waypoint.Position) / waypoint.Lane.Way.Segment.SpeedLimits.MaximumSpeed;
}
else if (waypoint.WaypointId.Number != 1)
{
// get closest partition
ArbiterWaypoint tmpI = this.GetClosestWaypoint(currentPosition, Double.MaxValue);
initialCost = NavigationPenalties.ChangeLanes * Math.Abs(this.LaneId.Number - al.LaneId.Number);
initialCost += currentPosition.DistanceTo(tmpI.Position) / tmpI.Lane.Way.Segment.SpeedLimits.MaximumSpeed;
}
else
{
// get closest partition
ArbiterWaypoint tmpI = this.GetClosestWaypoint(currentPosition, Double.MaxValue);
ArbiterWaypoint tmpF = this.GetClosestWaypoint(waypoint.Position, Double.MaxValue);
initialCost = NavigationPenalties.ChangeLanes * Math.Abs(this.LaneId.Number - al.LaneId.Number);
initialCost += currentPosition.DistanceTo(tmpI.Position) / tmpI.Lane.Way.Segment.SpeedLimits.MaximumSpeed;
initialCost += this.TimeCostInLane(tmpI, tmpF, new List <ArbiterWaypoint>());
}
// loop while waypoint not null
while (waypoint != null)
{
if (waypoint.IsCheckpoint && (goal is ArbiterWaypoint) && ((ArbiterWaypoint)goal).WaypointId.Equals(waypoint.WaypointId))
{
double timeCost = initialCost + this.TimeCostInLane(initial, waypoint, ignorable);
DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
dpoi.DistanceToPoint = al.DistanceBetween(currentPosition, waypoint.Position);
dpoi.IsExit = false;
dpoi.IsGoal = true;
dpoi.PointOfInterest = waypoint;
dpoi.TimeCostToPoint = timeCost;
waypoints.Add(dpoi);
}
else if (waypoint.IsExit && !ignorable.Contains(waypoint))
{
double timeCost = initialCost + this.TimeCostInLane(initial, waypoint, ignorable);
DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
dpoi.DistanceToPoint = al.DistanceBetween(currentPosition, waypoint.Position);
dpoi.IsExit = true;
dpoi.IsGoal = false;
dpoi.PointOfInterest = waypoint;
dpoi.TimeCostToPoint = timeCost;
waypoints.Add(dpoi);
}
else if (waypoint.NextPartition == null && !ignorable.Contains(waypoint))
{
DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
dpoi.DistanceToPoint = al.DistanceBetween(currentPosition, waypoint.Position);
dpoi.IsExit = false;
dpoi.IsGoal = false;
dpoi.PointOfInterest = waypoint;
dpoi.TimeCostToPoint = Double.MaxValue;
waypoints.Add(dpoi);
}
waypoint = waypoint.NextPartition != null ? waypoint.NextPartition.Final : null;
}
}
}
}
return(waypoints);
}
/// <summary>
/// Gets time cost between two waypoints in the same lane
/// </summary>
/// <param name="initial"></param>
/// <param name="final"></param>
/// <returns></returns>
public double TimeCostInLane(ArbiterWaypoint initial, ArbiterWaypoint final, List<ArbiterWaypoint> ignorable)
{
ArbiterWaypoint current = initial;
double cost = ignorable.Contains(initial) ? 0.0 : initial.ExtraTimeCost;
while (current != null)
{
if (current.Equals(final))
{
return cost;
}
else
{
if(current.NextPartition != null)
cost += current.NextPartition.TimeCost();
if (current.NextPartition != null)
current = current.NextPartition.Final;
else
current = null;
}
}
return Double.MaxValue;
}
/// <summary>
/// Get waypoints from initial to final
/// </summary>
/// <param name="initial"></param>
/// <param name="final"></param>
/// <returns></returns>
public List<ArbiterWaypoint> WaypointsInclusive(ArbiterWaypoint initial, ArbiterWaypoint final)
{
List<ArbiterWaypoint> middle = new List<ArbiterWaypoint>();
int i = waypointList.IndexOf(initial);
int j = waypointList.IndexOf(final);
for (int k = i; k <= j; k++)
{
middle.Add(waypointList[k]);
}
return middle;
}
