/// <summary> /// Updates the vehicle state as well as other values /// </summary> /// <param name="vehicleUpdate"></param> public void Update(SceneEstimatorTrackedCluster update) { // update state this.Observed = update; // calculate other values such as involved areas }
/// <summary> /// Constructor /// </summary> /// <param name="vehicleId"></param> public VehicleAgent(SceneEstimatorTrackedCluster ov) { this.VehicleId = ov.id; this.StateMonitor = new StateMonitor(); this.QueuingState = new QueuingMonitor(); this.UpdateState(ov); this.BirthTimer(); }
public VehicleAgent(bool temp, bool temp2) { this.VehicleId = 5000; SceneEstimatorTrackedCluster setc = new SceneEstimatorTrackedCluster(); setc.isStopped = true; setc.relativePoints = new Coordinates[0]; this.StateMonitor = new StateMonitor(); this.QueuingState = new QueuingMonitor(); this.BirthTimer(); this.StateMonitor.Observed = setc; }
private static Object ProcessTrackedClusterMsg(BinaryReader br) { br.BaseStream.Position = 0; SceneEstimatorTrackedClusterCollection tcc = new SceneEstimatorTrackedClusterCollection(); int numClusters = br.ReadInt32(); tcc.clusters = new SceneEstimatorTrackedCluster[numClusters]; tcc.timestamp = br.ReadDouble(); Dictionary <SceneEstimatorTrackedCluster, List <Pair <UInt16, UInt16> > > partitions = new Dictionary <SceneEstimatorTrackedCluster, List <Pair <ushort, ushort> > >(); for (int i = 0; i < numClusters; i++) { tcc.clusters[i] = new SceneEstimatorTrackedCluster(); int numPoints = br.ReadInt32(); tcc.clusters[i].relativePoints = new Coordinates[numPoints]; int numClosestPartitions = br.ReadInt32(); tcc.clusters[i].closestPartitions = new SceneEstimatorClusterPartition[numClosestPartitions]; double closestX = (double)br.ReadSingle(); double closestY = (double)br.ReadSingle(); tcc.clusters[i].closestPoint = new Coordinates(closestX, closestY); tcc.clusters[i].speed = br.ReadSingle(); tcc.clusters[i].speedValid = br.ReadBoolean(); tcc.clusters[i].relativeheading = br.ReadSingle(); tcc.clusters[i].absoluteHeading = br.ReadSingle(); tcc.clusters[i].headingValid = br.ReadBoolean(); tcc.clusters[i].targetClass = (SceneEstimatorTargetClass)br.ReadInt32(); tcc.clusters[i].id = br.ReadInt32(); tcc.clusters[i].statusFlag = (SceneEstimatorTargetStatusFlag)br.ReadInt32(); tcc.clusters[i].isStopped = br.ReadBoolean(); List <Pair <UInt16, UInt16> > partition = new List <Pair <ushort, ushort> >(); //create the partitions (still mapped) for (int j = 0; j < numClosestPartitions; j++) { UInt16 partHashID = br.ReadUInt16(); UInt16 partProbFP = br.ReadUInt16(); partition.Add(new Pair <ushort, ushort>(partHashID, partProbFP)); } partitions.Add(tcc.clusters[i], partition); //create the points for (int j = 0; j < numPoints; j++) { Int16 tmpx = br.ReadInt16(); Int16 tmpy = br.ReadInt16(); tcc.clusters[i].relativePoints[j].X = (double)tmpx / 100.0; tcc.clusters[i].relativePoints[j].Y = (double)tmpy / 100.0; } } //now get the map int mapsize = br.ReadInt32(); Dictionary <UInt16, string> map = new Dictionary <ushort, string>(); for (int i = 0; i < mapsize; i++) { string s = br.ReadString(); UInt16 id = br.ReadUInt16(); map.Add(id, s); } //now reprocess the clusters and populate their closest partitions from the map we made for (int i = 0; i < numClusters; i++) { if (partitions.ContainsKey(tcc.clusters[i]) == false) { continue; } int numPart = partitions[tcc.clusters[i]].Count; tcc.clusters[i].closestPartitions = new SceneEstimatorClusterPartition[numPart]; int j = 0; SceneEstimatorTrackedCluster clust = tcc.clusters[i]; List <Pair <UInt16, UInt16> > death = partitions[clust]; foreach (Pair <UInt16, UInt16> entry in death) { //the pair is HASH ID and PROB(fp) if (map.ContainsKey(entry.Left) == false) { throw new InvalidDataException("Recieved an Invalid Mapping for Closest Partitions. Fatal."); } tcc.clusters[i].closestPartitions[j].partitionID = map[entry.Left]; tcc.clusters[i].closestPartitions[j].probability = (float)entry.Right / 65535.0f; if (tcc.clusters[i].closestPartitions[j].probability > 1.0f) { Console.WriteLine("Warning: Overflow on probability of partition: " + tcc.clusters[i].closestPartitions[j].probability.ToString()); tcc.clusters[i].closestPartitions[j].probability = 1.0f; } else if (tcc.clusters[i].closestPartitions[j].probability < 0.0f) { Console.WriteLine("Warning: Underflow on probability of partition: " + tcc.clusters[i].closestPartitions[j].probability.ToString()); tcc.clusters[i].closestPartitions[j].probability = 0.0f; } //if (tcc.clusters[i].closestPartitions[j].probability != 0) // Console.WriteLine("stuff! " + tcc.clusters[i].closestPartitions[j].probability.ToString()); j++; } } if (br.BaseStream.Position != br.BaseStream.Length) { Console.WriteLine("Warning: Incomplete parse of received tracked cluster message. length is " + br.BaseStream.Length + ", go to " + br.BaseStream.Position + "."); } return(tcc); }
/// <summary> /// Populates mapping of vehicles we want to track given state /// </summary> /// <param name="vehicles"></param> public void PopulateValidVehicles(SceneEstimatorTrackedClusterCollection vehicles) { TacticalDirector.NewVehicles = new List <VehicleAgent>(); TacticalDirector.OccludedVehicles = new Dictionary <int, VehicleAgent>(); if (TacticalDirector.ValidVehicles == null) { TacticalDirector.ValidVehicles = new Dictionary <int, VehicleAgent>(); } List <int> toRemove = new List <int>(); foreach (VehicleAgent va in TacticalDirector.ValidVehicles.Values) { bool found = false; for (int i = 0; i < vehicles.clusters.Length; i++) { if (vehicles.clusters[i].id.Equals(va.VehicleId)) { found = true; } } if (!found) { toRemove.Add(va.VehicleId); } } foreach (int r in toRemove) { TacticalDirector.ValidVehicles.Remove(r); } for (int i = 0; i < vehicles.clusters.Length; i++) { SceneEstimatorTrackedCluster ov = vehicles.clusters[i]; bool clusterStopped = ov.isStopped; if (ov.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_ACTIVE || (ov.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_OCCLUDED_PART && !clusterStopped) || (ov.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_OCCLUDED_FULL && !clusterStopped)) { if (ov.targetClass == SceneEstimatorTargetClass.TARGET_CLASS_CARLIKE) { if (TacticalDirector.ValidVehicles.ContainsKey(ov.id)) { TacticalDirector.ValidVehicles[ov.id].StateMonitor.Observed = ov; } else { VehicleAgent ovNew = new VehicleAgent(ov); TacticalDirector.ValidVehicles.Add(ovNew.VehicleId, ovNew); TacticalDirector.NewVehicles.Add(ovNew); } } else { if (TacticalDirector.ValidVehicles.ContainsKey(ov.id)) { TacticalDirector.ValidVehicles.Remove(ov.id); } } } else { if (TacticalDirector.ValidVehicles.ContainsKey(ov.id)) { TacticalDirector.ValidVehicles.Remove(ov.id); } } if ((ov.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_OCCLUDED_PART && clusterStopped) || (ov.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_OCCLUDED_FULL && clusterStopped)) { TacticalDirector.OccludedVehicles.Add(ov.id, new VehicleAgent(ov)); } } }
public SensedVehicleDisplay(SceneEstimatorTrackedCluster trackedCluster) { this.trackedCluster = trackedCluster; }
/// <summary> /// Updates the vehicle's monitor /// </summary> /// <param name="vehicleUpdate"></param> /// <param name="speed"></param> public void UpdateState(SceneEstimatorTrackedCluster update) { this.StateMonitor.Update(update); }
/// <summary> /// Get the vehicles from the world and put them into sensors form /// </summary> /// <param name="ours"></param> /// <returns></returns> public SceneEstimatorTrackedClusterCollection VehiclesFromWorld(SimVehicleId ours, double ts) { // vehicle list List <SceneEstimatorTrackedCluster> vehicles = new List <SceneEstimatorTrackedCluster>(); // get our vehicle SimVehicleState ourVS = null; foreach (SimVehicleState svs in this.worldState.Vehicles.Values) { if (svs.VehicleID.Equals(ours)) { ourVS = svs; } } // generate "tracked" clusters foreach (SimVehicleState svs in this.worldState.Vehicles.Values) { // don't inclue our vehicle if (!svs.VehicleID.Equals(ours)) { // construct cluster SceneEstimatorTrackedCluster setc = new SceneEstimatorTrackedCluster(); // set heading valid setc.headingValid = true; // closest point Coordinates closestPoint = this.ClosestToPolygon(this.VehiclePolygon(svs), ourVS.Position); setc.closestPoint = closestPoint; // stopped bool isStopped = Math.Abs(svs.Speed) < 0.01; setc.isStopped = isStopped; // speed float speed = (float)Math.Abs(svs.Speed); setc.speed = speed; setc.speedValid = svs.SpeedValid; // absolute heading float absHeading = (float)(svs.Heading.ArcTan); setc.absoluteHeading = absHeading; // relative heading float relHeading = absHeading - (float)(ourVS.Heading.ArcTan); setc.relativeheading = relHeading; // set target class setc.targetClass = SceneEstimatorTargetClass.TARGET_CLASS_CARLIKE; // set id setc.id = svs.VehicleID.Number; // cluster partitions SceneEstimatorClusterPartition[] partitions = this.GetClusterPartitions(svs, ourVS); setc.closestPartitions = partitions; // state setc.statusFlag = SceneEstimatorTargetStatusFlag.TARGET_STATE_ACTIVE; // raw points Coordinates[] points = this.VehiclePointsRelative(svs, ourVS.Position, ourVS.Heading); setc.relativePoints = points; // add vehicles.Add(setc); } } // array of clusters SceneEstimatorTrackedClusterCollection setcc = new SceneEstimatorTrackedClusterCollection(); setcc.clusters = vehicles.ToArray(); setcc.timestamp = ts; // return the clusters return(setcc); }