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