/// <summary> /// Constructor /// </summary> /// <param name="key"></param> /// <param name="value"></param> public AreaProbability(IVehicleArea key, double value) { this.Key = key; this.Value = value; }
/// <summary> /// Populates mapping of areas to vehicles /// </summary> public void PopulateAreaVehicles() { TacticalDirector.VehicleAreas = new Dictionary <IVehicleArea, List <VehicleAgent> >(); VehicleState vs = CoreCommon.Communications.GetVehicleState(); Circle circ = new Circle(TahoeParams.T + 0.3, new Coordinates()); Polygon conv = circ.ToPolygon(32); Circle circ1 = new Circle(TahoeParams.T + 0.6, new Coordinates()); Polygon conv1 = circ1.ToPolygon(32); Circle circ2 = new Circle(TahoeParams.T + 1.4, new Coordinates()); Polygon conv2 = circ2.ToPolygon(32); foreach (VehicleAgent va in TacticalDirector.ValidVehicles.Values) { #region Standard Areas List <AreaProbability> AreaProbabilities = new List <AreaProbability>(); #region Add up probablities for (int i = 0; i < va.StateMonitor.Observed.closestPartitions.Length; i++) { SceneEstimatorClusterPartition secp = va.StateMonitor.Observed.closestPartitions[i]; if (CoreCommon.RoadNetwork.VehicleAreaMap.ContainsKey(secp.partitionID)) { IVehicleArea iva = CoreCommon.RoadNetwork.VehicleAreaMap[secp.partitionID]; bool found = false; for (int j = 0; j < AreaProbabilities.Count; j++) { AreaProbability ap = AreaProbabilities[j]; if (ap.Key.Equals(iva)) { ap.Value = ap.Value + secp.probability; found = true; } } if (!found) { AreaProbabilities.Add(new AreaProbability(iva, secp.probability)); } } else { ArbiterOutput.Output("Core Intelligence thread caught exception, partition: " + secp.partitionID + " not found in Vehicle Area Map"); } } #endregion #region Assign if (AreaProbabilities.Count > 0) { double rP = 0.0; foreach (AreaProbability ap in AreaProbabilities) { if (ap.Key is ArbiterLane) { rP += ap.Value; } } if (rP > 0.1) { foreach (AreaProbability ap in AreaProbabilities) { if (ap.Key is ArbiterLane) { // get lane ArbiterLane al = (ArbiterLane)ap.Key; // probability says in area double vP = ap.Value / rP; if (vP > 0.3) { #region Check if obstacle enough in area bool ok = false; if (ap.Key is ArbiterLane) { Coordinates closest = va.ClosestPointToLine(al.LanePath(), vs).Value; // dist to closest double distanceToClosest = vs.Front.DistanceTo(closest); // get our dist to closest if (30.0 < distanceToClosest && distanceToClosest < (30.0 + ((5.0 / 2.24) * Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value)))) { if (al.LanePolygon != null) { ok = this.VehicleExistsInsidePolygon(va, al.LanePolygon, vs); } else { ok = al.LanePath().GetClosestPoint(closest).Location.DistanceTo(closest) < al.Width / 2.0; } } else if (distanceToClosest <= 30.0) { if (al.LanePolygon != null) { #warning Darpa is an asshole if (!va.IsStopped && this.VehicleAllInsidePolygon(va, al.LanePolygon, vs)) { ok = true; } else { if (va.IsStopped) { // check vehicle is in a safety zone bool isInSafety = false; try { foreach (ArbiterIntersection ai in CoreCommon.RoadNetwork.ArbiterIntersections.Values) { if (ai.IntersectionPolygon.IsInside(va.ClosestPosition)) { isInSafety = true; } } foreach (ArbiterSafetyZone asz in al.SafetyZones) { if (asz.IsInSafety(va.ClosestPosition)) { isInSafety = true; } } } catch (Exception) { } if (isInSafety) { if (!this.VehiclePassableInPolygon(al, va, al.LanePolygon, vs, conv1)) { ok = true; } } else { if (!this.VehiclePassableInPolygon(al, va, al.LanePolygon, vs, conv)) { ok = true; } } } else { if (!this.VehiclePassableInPolygon(al, va, al.LanePolygon, vs, conv2)) { ok = true; } } } } else { ok = al.LanePath().GetClosestPoint(closest).Location.DistanceTo(closest) < al.Width / 2.0; } } else { ok = true; } } #endregion if (ok) { if (TacticalDirector.VehicleAreas.ContainsKey(ap.Key)) { TacticalDirector.VehicleAreas[ap.Key].Add(va); } else { List <VehicleAgent> vas = new List <VehicleAgent>(); vas.Add(va); TacticalDirector.VehicleAreas.Add(ap.Key, vas); } } } } } } } #endregion #endregion #region Interconnect Area Mappings foreach (ArbiterInterconnect ai in CoreCommon.RoadNetwork.ArbiterInterconnects.Values) { if (ai.TurnPolygon.IsInside(va.StateMonitor.Observed.closestPoint)) { if (TacticalDirector.VehicleAreas.ContainsKey(ai) && !TacticalDirector.VehicleAreas[ai].Contains(va)) { TacticalDirector.VehicleAreas[ai].Add(va); } else { List <VehicleAgent> vas = new List <VehicleAgent>(); vas.Add(va); TacticalDirector.VehicleAreas.Add(ai, vas); } // check if uturn if (ai.TurnDirection == ArbiterTurnDirection.UTurn && ai.InitialGeneric is ArbiterWaypoint && ai.FinalGeneric is ArbiterWaypoint) { // get the lanes the uturn is a part of ArbiterLane initialLane = ((ArbiterWaypoint)ai.InitialGeneric).Lane; ArbiterLane targetLane = ((ArbiterWaypoint)ai.FinalGeneric).Lane; if (TacticalDirector.VehicleAreas.ContainsKey(initialLane)) { if (!TacticalDirector.VehicleAreas[initialLane].Contains(va)) { TacticalDirector.VehicleAreas[initialLane].Add(va); } } else { List <VehicleAgent> vas = new List <VehicleAgent>(); vas.Add(va); TacticalDirector.VehicleAreas.Add(initialLane, vas); } if (TacticalDirector.VehicleAreas.ContainsKey(targetLane)) { if (!TacticalDirector.VehicleAreas[targetLane].Contains(va)) { TacticalDirector.VehicleAreas[targetLane].Add(va); } } else { List <VehicleAgent> vas = new List <VehicleAgent>(); vas.Add(va); TacticalDirector.VehicleAreas.Add(targetLane, vas); } } } } #endregion } }
public string PartitionIdString() { string final = ""; #region Standard Areas List <AreaProbability> AreaProbabilities = new List <AreaProbability>(); Circle circ = new Circle(TahoeParams.T + 0.3, new Coordinates()); Polygon conv = circ.ToPolygon(32); Circle circ1 = new Circle(TahoeParams.T + 0.6, new Coordinates()); Polygon conv1 = circ1.ToPolygon(32); Circle circ2 = new Circle(TahoeParams.T + 1.4, new Coordinates()); Polygon conv2 = circ2.ToPolygon(32); for (int i = 0; i < this.trackedCluster.closestPartitions.Length; i++) { SceneEstimatorClusterPartition secp = this.trackedCluster.closestPartitions[i]; if (RemoraCommon.RoadNetwork.VehicleAreaMap.ContainsKey(secp.partitionID)) { IVehicleArea iva = RemoraCommon.RoadNetwork.VehicleAreaMap[secp.partitionID]; bool found = false; for (int j = 0; j < AreaProbabilities.Count; j++) { AreaProbability ap = AreaProbabilities[j]; if (ap.Key.Equals(iva)) { ap.Value = ap.Value + secp.probability; found = true; } } if (!found) { AreaProbabilities.Add(new AreaProbability(iva, secp.probability)); } } else { RemoraOutput.WriteLine("Remora caught exception, partition: " + secp.partitionID + " not found in Vehicle Area Map", OutputType.Remora); } } if (AreaProbabilities.Count > 0) { double rP = 0.0; foreach (AreaProbability ap in AreaProbabilities) { if (ap.Key is ArbiterLane) { rP += ap.Value; } } if (rP > 0.1) { foreach (AreaProbability ap in AreaProbabilities) { if (ap.Key is ArbiterLane) { // proabbility says in area double vP = ap.Value / rP; if (vP > 0.3) { #region Check if obstacle enough in area bool ok = false; if (ap.Key is ArbiterLane) { VehicleState vs = RemoraCommon.Communicator.GetVehicleState(); ArbiterLane al = (ArbiterLane)ap.Key; Coordinates closest = this.ClosestPointToLine(al.LanePath(), vs).Value; // dist to closest double distanceToClosest = vs.Front.DistanceTo(closest); // get our dist to closest if (30.0 < distanceToClosest && distanceToClosest < (30.0 + ((5.0 / 2.24) * Math.Abs(RemoraCommon.Communicator.GetVehicleSpeed().Value)))) { if (al.LanePolygon != null) { ok = this.VehicleExistsInsidePolygon(al.LanePolygon, vs); } else { ok = al.LanePath().GetClosestPoint(closest).Location.DistanceTo(closest) < al.Width / 2.0; } } else if (distanceToClosest <= 30.0) { if (al.LanePolygon != null) { if (!this.trackedCluster.isStopped && this.VehicleAllInsidePolygon(al.LanePolygon, vs)) { ok = true; } else { if (this.trackedCluster.isStopped) { bool isInSafety = false; foreach (ArbiterIntersection ai in RemoraCommon.RoadNetwork.ArbiterIntersections.Values) { if (ai.IntersectionPolygon.IsInside(this.trackedCluster.closestPoint)) { isInSafety = true; } } foreach (ArbiterSafetyZone asz in al.SafetyZones) { if (asz.IsInSafety(this.trackedCluster.closestPoint)) { isInSafety = true; } } if (isInSafety) { if (!this.VehiclePassableInPolygon(al, al.LanePolygon, vs, conv1)) { ok = true; } } else { if (!this.VehiclePassableInPolygon(al, al.LanePolygon, vs, conv)) { ok = true; } } } else { if (!this.VehiclePassableInPolygon(al, al.LanePolygon, vs, conv2)) { ok = true; } } } } else { ok = al.LanePath().GetClosestPoint(closest).Location.DistanceTo(closest) < al.Width / 2.0; } } else { ok = true; } if (ok) { final = final + ap.Key.ToString() + ": " + vP.ToString("F4") + "\n"; } } #endregion } } } } } #endregion #region Interconnect Area Mappings foreach (ArbiterInterconnect ai in RemoraCommon.RoadNetwork.ArbiterInterconnects.Values) { if (ai.TurnPolygon.IsInside(this.trackedCluster.closestPoint)) { final = final + ai.ToString() + "\n"; if (ai.TurnDirection == ArbiterTurnDirection.UTurn && ai.InitialGeneric is ArbiterWaypoint && ai.FinalGeneric is ArbiterWaypoint) { // initial ArbiterLane initialLane = ((ArbiterWaypoint)ai.InitialGeneric).Lane; ArbiterLane targetLane = ((ArbiterWaypoint)ai.FinalGeneric).Lane; final = final + "UTurn2Lane: " + initialLane.ToString() + " & " + targetLane.ToString() + "\n"; } } } #endregion #region Intersections foreach (ArbiterIntersection aInt in RemoraCommon.RoadNetwork.ArbiterIntersections.Values) { if (aInt.IntersectionPolygon.IsInside(this.trackedCluster.closestPoint)) { final = final + "Inter: " + aInt.ToString() + "\n"; } } #endregion return(final); }
public IntersectionInvolved(IVehicleArea area) { this.Exit = null; this.Area = area; this.TurnDirection = ArbiterTurnDirection.Unknown; }
/// <summary> /// Constructor /// </summary> /// <param name="exit"></param> /// <param name="area"></param> /// <param name="turnDirection"></param> public IntersectionInvolved(ITraversableWaypoint exit, IVehicleArea area, ArbiterTurnDirection turnDirection) { this.Exit = exit; this.Area = area; this.TurnDirection = turnDirection; }