private void IntersectionReParseAllButton_Click(object sender, EventArgs e) { // tool IntersectionPulloutTool ipt = new IntersectionPulloutTool(this.arn, rd, ed, false); // save old ArbiterIntersection[] ais = new ArbiterIntersection[this.arn.ArbiterIntersections.Count]; this.arn.ArbiterIntersections.Values.CopyTo(ais, 0); // remove all old /*for(int i =0; i < ais.Length; i++) { ArbiterIntersection ai = ais[i]; this.rd.displayObjects.Remove(ai); ai.RoadNetwork.DisplayObjects.Remove(ai); ai.RoadNetwork.ArbiterIntersections.Remove(ai.IntersectionId); foreach (ITraversableWaypoint aw in ai.AllExits.Values) ai.RoadNetwork.IntersectionLookup.Remove(aw.AreaSubtypeWaypointId); }*/ // refinalize for (int j = 0; j < ais.Length; j++) { ArbiterIntersection ai = ais[j]; ipt.FinalizeIntersection(ai.IntersectionPolygon.Inflate(3.0), ai); } }
public IntersectionStartupState(ArbiterIntersection ai) { this.Intersection = ai; }
/// <summary> /// Startup the vehicle from a certain position, pass the next state back, /// and initialize the lane agent if possible /// </summary> /// <param name="vehicleState"></param> /// <returns></returns> public IState Startup(VehicleState vehicleState, CarMode carMode) { // check car mode if (carMode == CarMode.Run) { // check areas ArbiterLane al = CoreCommon.RoadNetwork.ClosestLane(vehicleState.Front); ArbiterZone az = CoreCommon.RoadNetwork.InZone(vehicleState.Front); ArbiterIntersection ain = CoreCommon.RoadNetwork.InIntersection(vehicleState.Front); ArbiterInterconnect ai = CoreCommon.RoadNetwork.ClosestInterconnect(vehicleState.Front, vehicleState.Heading); if (az != null) { // special zone startup return(new ZoneStartupState(az)); } if (ain != null) { if (al != null) { // check lane stuff PointOnPath lanePoint = al.PartitionPath.GetClosest(vehicleState.Front); // get distance from front of car double dist = lanePoint.pt.DistanceTo(vehicleState.Front); // check dist to lane if (dist < al.Width + 1.0) { // check orientation relative to lane Coordinates laneVector = al.GetClosestPartition(vehicleState.Front).Vector().Normalize(); // get our heading Coordinates ourHeading = vehicleState.Heading.Normalize(); // forwards or backwards bool forwards = true; // check dist to each other if (laneVector.DistanceTo(ourHeading) > Math.Sqrt(2.0)) { // not going forwards forwards = false; } // stay in lane if forwards if (forwards) { ArbiterOutput.Output("Starting up in lane: " + al.ToString()); return(new StayInLaneState(al, new Probability(0.7, 0.3), true, CoreCommon.CorePlanningState)); } else { // Opposing lane return(new OpposingLanesState(al, true, CoreCommon.CorePlanningState, vehicleState)); } } } // startup intersection state return(new IntersectionStartupState(ain)); } if (al != null) { // get a startup chute ArbiterLanePartition startupChute = this.GetStartupChute(vehicleState); // check if in a startup chute if (startupChute != null && !startupChute.IsInside(vehicleState.Front)) { ArbiterOutput.Output("Starting up in chute: " + startupChute.ToString()); return(new StartupOffChuteState(startupChute)); } // not in a startup chute else { PointOnPath lanePoint = al.PartitionPath.GetClosest(vehicleState.Front); // get distance from front of car double dist = lanePoint.pt.DistanceTo(vehicleState.Front); // check orientation relative to lane Coordinates laneVector = al.GetClosestPartition(vehicleState.Front).Vector().Normalize(); // get our heading Coordinates ourHeading = vehicleState.Heading.Normalize(); // forwards or backwards bool forwards = true; // check dist to each other if (laneVector.DistanceTo(ourHeading) > Math.Sqrt(2.0)) { // not going forwards forwards = false; } // stay in lane if forwards if (forwards) { ArbiterOutput.Output("Starting up in lane: " + al.ToString()); return(new StayInLaneState(al, new Probability(0.7, 0.3), true, CoreCommon.CorePlanningState)); } else { // opposing return(new OpposingLanesState(al, true, CoreCommon.CorePlanningState, vehicleState)); } } } // fell out ArbiterOutput.Output("Cannot find area to startup in"); return(CoreCommon.CorePlanningState); } else { return(CoreCommon.CorePlanningState); } }
/// <summary> /// Constructor /// </summary> /// <param name="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> /// Check if we can go /// </summary> /// <param name="vs"></param> private bool CanGo(VehicleState vs) { #region Moving Vehicles Inside Turn // check if we can still go through this turn if (TacticalDirector.VehicleAreas.ContainsKey(this.turn)) { // get the subpath of the interconnect we care about LinePath.PointOnPath frontPos = this.turn.InterconnectPath.GetClosestPoint(vs.Front); LinePath aiSubpath = this.turn.InterconnectPath.SubPath(frontPos, this.turn.InterconnectPath.EndPoint); if (aiSubpath.PathLength > 4.0) { aiSubpath = aiSubpath.SubPath(aiSubpath.StartPoint, aiSubpath.PathLength - 2.0); // get vehicles List <VehicleAgent> turnVehicles = TacticalDirector.VehicleAreas[this.turn]; // loop vehicles foreach (VehicleAgent va in turnVehicles) { // check if inside turn LinePath.PointOnPath vaPop = aiSubpath.GetClosestPoint(va.ClosestPosition); if (!va.IsStopped && this.turn.TurnPolygon.IsInside(va.ClosestPosition) && !vaPop.Equals(aiSubpath.StartPoint) && !vaPop.Equals(aiSubpath.EndPoint)) { ArbiterOutput.Output("Vehicle seen inside our turn: " + va.ToString() + ", stopping"); return(false); } } } } #endregion // test if this turn is part of an intersection if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(this.turn.InitialGeneric.AreaSubtypeWaypointId)) { // intersection ArbiterIntersection inter = CoreCommon.RoadNetwork.IntersectionLookup[this.turn.InitialGeneric.AreaSubtypeWaypointId]; // check if priority lanes exist for this interconnect if (inter.PriorityLanes.ContainsKey(this.turn)) { // get all the default priority lanes List <IntersectionInvolved> priorities = inter.PriorityLanes[this.turn]; // get the subpath of the interconnect we care about //LinePath.PointOnPath frontPos = this.turn.InterconnectPath.GetClosestPoint(vs.Front); LinePath aiSubpath = new LinePath(new List <Coordinates>(new Coordinates[] { vs.Front, this.turn.FinalGeneric.Position })); //this.turn.InterconnectPath.SubPath(frontPos, this.turn.InterconnectPath.EndPoint); // check if path ended if (aiSubpath.Count < 2) { return(true); } // determine all of the new priority lanes List <IntersectionInvolved> updatedPriorities = new List <IntersectionInvolved>(); #region Determine new priority areas given position // loop through old priorities foreach (IntersectionInvolved ii in priorities) { // check ii lane if (ii.Area is ArbiterLane) { #region Lane Intersects Turn Path w/ Point of No Return // check if the waypoint is not the last waypoint in the lane if (ii.Exit == null || ((ArbiterWaypoint)ii.Exit).NextPartition != null) { // check where line intersects path Coordinates?intersect = this.LaneIntersectsPath(ii, aiSubpath, this.turn.FinalGeneric); // check for an intersection if (intersect.HasValue) { // distance to intersection double distanceToIntersection = (intersect.Value.DistanceTo(vs.Front) + ((ArbiterLane)ii.Area).LanePath().GetClosestPoint(vs.Front).Location.DistanceTo(vs.Front)) / 2.0; // determine if we can stop before or after the intersection double distanceToStoppage = RoadToolkit.DistanceToStop(CoreCommon.Communications.GetVehicleSpeed().Value); // check dist to intersection > distance to stoppage if (distanceToIntersection > distanceToStoppage) { // add updated priority updatedPriorities.Add(new IntersectionInvolved(new ArbiterWaypoint(intersect.Value, new ArbiterWaypointId(0, ((ArbiterLane)ii.Area).LaneId)), ii.Area, ArbiterTurnDirection.Straight)); } else { ArbiterOutput.Output("Passed point of No Return for Lane: " + ii.Area.ToString()); } } } #endregion // we know there is an exit and it is the last waypoint of the segment, fuxk! else { #region Turns Intersect // get point to look at if exists ArbiterInterconnect interconnect; Coordinates? intersect = this.TurnIntersects(aiSubpath, ii.Exit, out interconnect); // check for the intersect if (intersect.HasValue) { ArbiterLane al = (ArbiterLane)ii.Area; LinePath lp = al.LanePath().SubPath(al.LanePath().StartPoint, al.LanePath().GetClosestPoint(ii.Exit.Position)); lp.Add(interconnect.InterconnectPath.EndPoint.Location); // get our time to the intersection point //double ourTime = Math.Min(4.0, Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.001 ? aiSubpath.PathLength / 1.0 : aiSubpath.PathLength / Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value)); // get our time to the intersection point double ourSpeed = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value); double stoppedTime = ourSpeed < 1.0 ? 1.5 : 0.0; double extraTime = 1.5; double interconnectTime = aiSubpath.PathLength / this.turn.MaximumDefaultSpeed; double ourTime = Math.Min(6.5, stoppedTime + extraTime + interconnectTime); // get closest vehicle in that lane to the intersection List <VehicleAgent> toLook = new List <VehicleAgent>(); if (TacticalDirector.VehicleAreas.ContainsKey(ii.Area)) { foreach (VehicleAgent tmpVa in TacticalDirector.VehicleAreas[ii.Area]) { double upstreamDist = al.DistanceBetween(tmpVa.ClosestPosition, ii.Exit.Position); if (upstreamDist > 0 && tmpVa.PassedDelayedBirth) { toLook.Add(tmpVa); } } } if (TacticalDirector.VehicleAreas.ContainsKey(interconnect)) { toLook.AddRange(TacticalDirector.VehicleAreas[interconnect]); } // check length of stuff to look at if (toLook.Count > 0) { foreach (VehicleAgent va in toLook) { // distance along path to location of intersect double distToIntersect = lp.DistanceBetween(lp.GetClosestPoint(va.ClosestPosition), lp.GetClosestPoint(aiSubpath.GetClosestPoint(va.ClosestPosition).Location)); double speed = va.Speed == 0.0 ? 0.01 : va.Speed; double vaTime = distToIntersect / Math.Abs(speed); if (vaTime > 0 && vaTime < ourTime) { ArbiterOutput.Output("va: " + va.ToString() + " CollisionTimer: " + vaTime.ToString("f2") + " < TimeUs: " + ourTime.ToString("F2") + ", NOGO"); return(false); } } } } #endregion } } } #endregion #region Updated Priority Intersection Code // loop through updated priorities bool updatedPrioritiesClear = true; foreach (IntersectionInvolved ii in updatedPriorities) { // lane ArbiterLane al = (ArbiterLane)ii.Area; // get our time to the intersection point double ourSpeed = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value); double stoppedTime = ourSpeed < 1.0 ? 1.5 : 0.0; double extraTime = 1.5; double interconnectTime = aiSubpath.PathLength / this.turn.MaximumDefaultSpeed; double ourTime = Math.Min(6.5, stoppedTime + extraTime + interconnectTime); // double outTime = Math.Min(4.0, Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.001 ? aiSubpath.PathLength / 1.0 : aiSubpath.PathLength / Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value)); // get closest vehicle in that lane to the intersection if (TacticalDirector.VehicleAreas.ContainsKey(ii.Area)) { // get lane vehicles List <VehicleAgent> vas = TacticalDirector.VehicleAreas[ii.Area]; // determine for all VehicleAgent closestLaneVa = null; double closestDistanceVa = Double.MaxValue; double closestTime = Double.MaxValue; foreach (VehicleAgent testVa in vas) { // check upstream double distance = al.DistanceBetween(testVa.ClosestPosition, ii.Exit.Position); // get speed double speed = testVa.Speed; double time = testVa.StateMonitor.Observed.speedValid ? distance / Math.Abs(speed) : distance / al.Way.Segment.SpeedLimits.MaximumSpeed; // check distance > 0 if (distance > 0) { // check if closer or none other exists if (closestLaneVa == null || time < closestTime) { closestLaneVa = testVa; closestDistanceVa = distance; closestTime = time; } } } // check if closest exists if (closestLaneVa != null) { // set va VehicleAgent va = closestLaneVa; double distance = closestDistanceVa; // check dist and birth time if (distance > 0 && va.PassedDelayedBirth) { // check time double speed = va.Speed == 0.0 ? 0.01 : va.Speed; double time = va.StateMonitor.Observed.speedValid ? distance / Math.Abs(speed) : distance / al.Way.Segment.SpeedLimits.MaximumSpeed; // too close if (!al.LanePolygon.IsInside(CoreCommon.Communications.GetVehicleState().Front) && distance < 25 && (!va.StateMonitor.Observed.speedValid || !va.StateMonitor.Observed.isStopped) && CoreCommon.Communications.GetVehicleState().Front.DistanceTo(va.ClosestPosition) < 20) { ArbiterOutput.Output("Turn, NOGO, Lane: " + al.ToString() + " vehicle: " + va.ToString() + " possibly moving to close, stopping"); //return false; updatedPrioritiesClear = false; return(false); } else if (time > 0 && time < ourTime) { ArbiterOutput.Output("Turn, NOGO, Lane: " + al.ToString() + ", va: " + va.ToString() + ", stopped: " + va.IsStopped.ToString() + ", timeUs: " + ourTime.ToString("f2") + ", timeThem: " + time.ToString("f2")); //return false; updatedPrioritiesClear = false; return(false); } else { ArbiterOutput.Output("Turn, CANGO, Lane: " + al.ToString() + ", va: " + va.ToString() + ", stopped: " + va.IsStopped.ToString() + ", timeUs: " + ourTime.ToString("f2") + ", timeThem: " + time.ToString("f2")); //return true; } } } else { ArbiterOutput.Output("Turn, CANGO, Lane: " + al.ToString() + " has no traffic vehicles"); } } } return(updatedPrioritiesClear); #endregion } } // fall through fine to go ArbiterOutput.Output("In Turn, CAN GO, Clear of vehicles upstream"); return(true); }