/// <summary>
/// Pupulates all the priority monitors given the information we know right at this moment
/// </summary>
public void PopulateMonitor(IArbiterWaypoint ourExit)
{
// 1. Generate dependencies for each lane
// 2. Assign stopped vehicles at stops to stop lanes
// 3. Given dependencies populate the stop queues for each stopped vehicle
// 4. Assign stopped vehicles close to exits in priority lanes to their priority monitor
// 5. given our exit assign our dependencies
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initial"></param>
/// <param name="final"></param>
public ArbiterInterconnect(IArbiterWaypoint initial, IArbiterWaypoint final) :
base(false, null, false, null, new List <IConnectAreaWaypoints>(), initial, final)
{
this.initialWaypoint = initial;
this.finalWaypoint = final;
this.InterconnectId = new ArbiterInterconnectId(initialWaypoint.AreaSubtypeWaypointId, finalWaypoint.AreaSubtypeWaypointId);
// create a path of the partition and get the closest
List <Coordinates> ips = new List <Coordinates>();
ips.Add(initial.Position);
ips.Add(final.Position);
this.InterconnectPath = new LinePath(ips);
// nav edge stuff
this.Contained.Add(this);
this.blockage = new NavigationBlockage(0.0);
this.TurnPolygon = this.DefaultPoly();
this.InnerCoordinates = new List <Coordinates>();
}
/// <summary>
/// Get a road plan while setting partition costs very high
/// </summary>
/// <param name="partition"></param>
/// <param name="goal"></param>
/// <param name="blockAdjacent"></param>
/// <param name="c"></param>
/// <returns></returns>
public RoadPlan PlanRoadOppositeWithoutPartition(ArbiterLanePartition partition, ArbiterLanePartition opposite, IArbiterWaypoint goal, bool blockAdjacent, Coordinates c, bool sameWay)
{
KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> > tmpCurrentTimes = currentTimes;
this.currentTimes = new KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> >();
RoadPlan rp = null;
if (!blockAdjacent)
{
NavigationBlockage nb = partition.Blockage;
NavigationBlockage tmp = new NavigationBlockage(double.MaxValue);
partition.Blockage = tmp;
rp = this.PlanNavigableArea(partition.Lane, c, goal, new List <ArbiterWaypoint>());
partition.Blockage = nb;
}
else
{
// save
List <KeyValuePair <ArbiterLanePartition, NavigationBlockage> > savedBlockages = new List <KeyValuePair <ArbiterLanePartition, NavigationBlockage> >();
// set
savedBlockages.Add(new KeyValuePair <ArbiterLanePartition, NavigationBlockage>(partition, partition.Blockage));
// create new
NavigationBlockage anewerBlockage = new NavigationBlockage(Double.MaxValue);
anewerBlockage.BlockageExists = true;
partition.Blockage = anewerBlockage;
foreach (ArbiterLanePartition alp in partition.NonLaneAdjacentPartitions)
{
if (alp.IsInside(c) && (!sameWay || (sameWay && partition.Lane.Way.Equals(alp.Lane.Way))))
{
savedBlockages.Add(new KeyValuePair <ArbiterLanePartition, NavigationBlockage>(alp, alp.Blockage));
// create new
NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);
newerBlockage.BlockageExists = true;
alp.Blockage = newerBlockage;
}
}
// plan
rp = this.PlanNavigableArea(opposite.Lane, c, goal, new List <ArbiterWaypoint>());
// restore
foreach (KeyValuePair <ArbiterLanePartition, NavigationBlockage> saved in savedBlockages)
{
saved.Key.Blockage = saved.Value;
}
}
// restore
this.currentTimes = tmpCurrentTimes;
// return
return(rp);
}
/// <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>
/// 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);
}
public static INavigableNode FilterGoal(VehicleState state)
{
// get goal
INavigableNode goal = CoreCommon.Mission.MissionCheckpoints.Count > 0 ?
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId] : null;
if (waitRemoveLastGoal && CoreCommon.Mission.MissionCheckpoints.Count != 1)
{
waitRemoveLastGoal = false;
}
// id
IArbiterWaypoint goalWp = null;
if (goal != null)
{
goalWp = CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId];
}
// check lane change or opposing
if (goal != null &&
(CoreCommon.CorePlanningState is OpposingLanesState && ((OpposingLanesState)CoreCommon.CorePlanningState).HitGoal(state, goal.Position, goalWp.AreaSubtypeWaypointId)) ||
(CoreCommon.CorePlanningState is ChangeLanesState && ((ChangeLanesState)CoreCommon.CorePlanningState).HitGoal(state, goal.Position, goalWp.AreaSubtypeWaypointId)))
{
if (CoreCommon.Mission.MissionCheckpoints.Count == 1)
{
waitRemoveLastGoal = true;
ArbiterOutput.Output("Waiting to remove last Checkpoint: " + goal.ToString());
}
else
{
// set hit
ArbiterOutput.Output("Reached Checkpoint: " + goal.ToString());
CoreCommon.Mission.MissionCheckpoints.Dequeue();
// update goal
goal = CoreCommon.Mission.MissionCheckpoints.Count > 0 ?
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId] : null;
}
}
else if (goal != null && CoreCommon.Mission.MissionCheckpoints.Count == 1 && waitRemoveLastGoal &&
(CoreCommon.CorePlanningState is StayInLaneState || CoreCommon.CorePlanningState is StayInSupraLaneState))
{
// set hit
ArbiterOutput.Output("Wait over, Reached Checkpoint: " + goal.ToString());
CoreCommon.Mission.MissionCheckpoints.Dequeue();
// update goal
goal = CoreCommon.Mission.MissionCheckpoints.Count > 0 ?
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId] : null;
}
// TODO implement full version of hit test
// check if we have hit the goal (either by being in opposing lane or going to opposing and next to it or in lane and pass over it
else if (goal != null)
{
bool reachedCp = false;
if (CoreCommon.CorePlanningState is StayInLaneState)
{
StayInLaneState sils = (StayInLaneState)CoreCommon.CorePlanningState;
if (goal is ArbiterWaypoint && ((ArbiterWaypoint)goal).Lane.Equals(sils.Lane))
{
if (CoreCommon.Mission.MissionCheckpoints.Count != 1)
{
double distanceAlong = sils.Lane.DistanceBetween(state.Front, goal.Position);
if (Math.Abs(distanceAlong) < 1.5 + (1.5 * CoreCommon.Communications.GetVehicleSpeed().Value) / 5.0)
{
reachedCp = true;
}
}
else
{
double distanceAlong = sils.Lane.DistanceBetween(state.Front, goal.Position);
double distanceAlong2 = sils.Lane.DistanceBetween(state.Position, goal.Position);
if (CoreCommon.Communications.GetVehicleSpeed().Value < 0.005 && Math.Abs(distanceAlong) < 0.3 ||
CoreCommon.Communications.GetVehicleState().VehiclePolygon.IsInside(goal.Position) ||
(distanceAlong <= 0.0 && distanceAlong2 >= 0))
{
reachedCp = true;
}
}
}
}
else if (CoreCommon.CorePlanningState is ChangeLanesState)
{
ChangeLanesState cls = (ChangeLanesState)CoreCommon.CorePlanningState;
if (cls.Parameters.Initial.Way.Equals(cls.Parameters.Target.Way) &&
goal is ArbiterWaypoint && ((ArbiterWaypoint)goal).Lane.Equals(cls.Parameters.Target))
{
double distanceAlong = cls.Parameters.Target.DistanceBetween(state.Front, goal.Position);
if (Math.Abs(distanceAlong) < 1.5 + (1.5 * CoreCommon.Communications.GetVehicleSpeed().Value) / 5.0)
{
reachedCp = true;
ArbiterOutput.Output("Removed goal changing lanes");
}
}
}
if (reachedCp)
{
// set hit
ArbiterOutput.Output("Reached Checkpoint: " + goal.ToString());
CoreCommon.Mission.MissionCheckpoints.Dequeue();
// update goal
goal = CoreCommon.Mission.MissionCheckpoints.Count > 0 ?
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId] : null;
}
}
// set goal info
CoreCommon.CurrentInformation.RouteCheckpoint = CoreCommon.Mission.MissionCheckpoints.Count > 0 ? goal.ToString() : "NONE";
CoreCommon.CurrentInformation.GoalsRemaining = CoreCommon.Mission.MissionCheckpoints.Count.ToString();
CoreCommon.CurrentInformation.RouteCheckpointId = CoreCommon.Mission.MissionCheckpoints.Count > 0 ? CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString() : "NONE";
// return current
return(goal);
}
/// <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>
/// Pupulates all the priority monitors given the information we know right at this moment
/// </summary>
public void PopulateMonitor(IArbiterWaypoint ourExit)
{
// 1. Generate dependencies for each lane
// 2. Assign stopped vehicles at stops to stop lanes
// 3. Given dependencies populate the stop queues for each stopped vehicle
// 4. Assign stopped vehicles close to exits in priority lanes to their priority monitor
// 5. given our exit assign our dependencies
}
/// <summary>
/// Get a road plan while setting partition costs very high
/// </summary>
/// <param name="partition"></param>
/// <param name="goal"></param>
/// <param name="blockAdjacent"></param>
/// <param name="c"></param>
/// <returns></returns>
public RoadPlan PlanRoadOppositeWithoutPartition(ArbiterLanePartition partition, ArbiterLanePartition opposite, IArbiterWaypoint goal, bool blockAdjacent, Coordinates c, bool sameWay)
{
KeyValuePair<int, Dictionary<ArbiterWaypointId, DownstreamPointOfInterest>> tmpCurrentTimes = currentTimes;
this.currentTimes = new KeyValuePair<int, Dictionary<ArbiterWaypointId, DownstreamPointOfInterest>>();
RoadPlan rp = null;
if (!blockAdjacent)
{
NavigationBlockage nb = partition.Blockage;
NavigationBlockage tmp = new NavigationBlockage(double.MaxValue);
partition.Blockage = tmp;
rp = this.PlanNavigableArea(partition.Lane, c, goal, new List<ArbiterWaypoint>());
partition.Blockage = nb;
}
else
{
// save
List<KeyValuePair<ArbiterLanePartition, NavigationBlockage>> savedBlockages = new List<KeyValuePair<ArbiterLanePartition, NavigationBlockage>>();
// set
savedBlockages.Add(new KeyValuePair<ArbiterLanePartition,NavigationBlockage>(partition, partition.Blockage));
// create new
NavigationBlockage anewerBlockage = new NavigationBlockage(Double.MaxValue);
anewerBlockage.BlockageExists = true;
partition.Blockage = anewerBlockage;
foreach (ArbiterLanePartition alp in partition.NonLaneAdjacentPartitions)
{
if (alp.IsInside(c) && (!sameWay || (sameWay && partition.Lane.Way.Equals(alp.Lane.Way))))
{
savedBlockages.Add(new KeyValuePair<ArbiterLanePartition,NavigationBlockage>(alp, alp.Blockage));
// create new
NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);
newerBlockage.BlockageExists = true;
alp.Blockage = newerBlockage;
}
}
// plan
rp = this.PlanNavigableArea(opposite.Lane, c, goal, new List<ArbiterWaypoint>());
// restore
foreach (KeyValuePair<ArbiterLanePartition, NavigationBlockage> saved in savedBlockages)
{
saved.Key.Blockage = saved.Value;
}
}
// restore
this.currentTimes = tmpCurrentTimes;
// return
return rp;
}
/// <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>
/// 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 priotiy lane determination
/// </summary>
/// <param name="ii"></param>
/// <param name="path"></param>
/// <param name="end"></param>
/// <returns></returns>
private Coordinates?LaneIntersectsPath(IntersectionInvolved ii, LinePath path, IArbiterWaypoint end)
{
ArbiterLane al = (ArbiterLane)ii.Area;
LinePath.PointOnPath current = path.StartPoint;
bool go = true;
while (go) // && !(current.Location.DistanceTo(path.EndPoint.Location) < 0.1))
{
Coordinates alClose = al.LanePath().GetClosestPoint(current.Location).Location;
double alDist = alClose.DistanceTo(current.Location);
if (alDist <= 0.05)
{
return(al.LanePath().GetClosestPoint(current.Location).Location);
}
if (current.Location.Equals(path.EndPoint.Location))
{
go = false;
}
current = path.AdvancePoint(current, 0.1);
}
/*if (ii.Exit != null)
* {
* ITraversableWaypoint laneExit = ii.Exit;
* foreach (ArbiterInterconnect tmpAi in laneExit.OutgoingConnections)
* {
* if (tmpAi.FinalGeneric.Equals(end))
* {
* return tmpAi.FinalGeneric.Position;
* }
* }
* }*/
return(null);
}