/// <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);
}
