private void ObstacleThread()
{
for (;;)
{
try {
SceneEstimatorUntrackedClusterCollection newUntrackedClusters = Interlocked.Exchange(ref currentUntrackedClusters, null);
SceneEstimatorTrackedClusterCollection newTrackedClusters = Interlocked.Exchange(ref currentTrackedClusters, null);
if (newUntrackedClusters == null && newTrackedClusters == null)
{
if (!Services.DebuggingService.StepMode)
{
newDataEvent.WaitOne();
}
else
{
Services.DebuggingService.WaitOnSequencer(typeof(ObstaclePipeline));
}
continue;
}
// check if we have a matching pair
if (newUntrackedClusters != null)
{
queuedUntrackedClusters = newUntrackedClusters;
}
if (newTrackedClusters != null)
{
haveReceivedTrackedClusters = true;
queuedTrackedClusters = newTrackedClusters;
}
if (queuedUntrackedClusters == null || (haveReceivedTrackedClusters && (queuedTrackedClusters == null || queuedTrackedClusters.timestamp != queuedUntrackedClusters.timestamp)))
{
continue;
}
Rect vehicleBox = DetermineVehicleEraseBox();
// load in the appropriate stuff to the occupancy grid
useOccupancyGrid = (Services.OccupancyGrid != null && !Services.OccupancyGrid.IsDisposed);
if (useOccupancyGrid)
{
double occup_ts = Services.OccupancyGrid.LoadNewestGrid();
if (occup_ts < 0)
{
useOccupancyGrid = false;
}
else
{
double delta_ts = occup_ts - queuedUntrackedClusters.timestamp;
Services.Dataset.ItemAs <double>("occupancy delta ts").Add(delta_ts, queuedUntrackedClusters.timestamp);
}
}
occupancyDeletedCount = 0;
List <Obstacle> trackedObstacles;
if (queuedTrackedClusters == null)
{
trackedObstacles = new List <Obstacle>();
}
else
{
trackedObstacles = ProcessTrackedClusters(queuedTrackedClusters, vehicleBox);
}
List <Obstacle> untrackedObstacles = ProcessUntrackedClusters(queuedUntrackedClusters, trackedObstacles, vehicleBox);
List <Obstacle> finalObstacles = FinalizeProcessing(trackedObstacles, untrackedObstacles, queuedUntrackedClusters.timestamp);
processedObstacles = new ObstacleCollection(queuedUntrackedClusters.timestamp, finalObstacles);
Services.Dataset.ItemAs <int>("occupancy deleted count").Add(occupancyDeletedCount, queuedUntrackedClusters.timestamp);
queuedUntrackedClusters = null;
queuedTrackedClusters = null;
if (Services.DebuggingService.StepMode)
{
Services.DebuggingService.SetCompleted(typeof(ObstaclePipeline));
}
Services.Dataset.MarkOperation("obstacle rate", LocalCarTimeProvider.LocalNow);
}
catch (Exception ex) {
OperationalTrace.WriteError("error processing obstacles: {0}", ex);
}
}
}
private void ObstacleThread()
{
for (;;) {
try {
SceneEstimatorUntrackedClusterCollection newUntrackedClusters = Interlocked.Exchange(ref currentUntrackedClusters, null);
SceneEstimatorTrackedClusterCollection newTrackedClusters = Interlocked.Exchange(ref currentTrackedClusters, null);
if (newUntrackedClusters == null && newTrackedClusters == null) {
if (!Services.DebuggingService.StepMode) {
newDataEvent.WaitOne();
}
else {
Services.DebuggingService.WaitOnSequencer(typeof(ObstaclePipeline));
}
continue;
}
// check if we have a matching pair
if (newUntrackedClusters != null) {
queuedUntrackedClusters = newUntrackedClusters;
}
if (newTrackedClusters != null) {
haveReceivedTrackedClusters = true;
queuedTrackedClusters = newTrackedClusters;
}
if (queuedUntrackedClusters == null || (haveReceivedTrackedClusters && (queuedTrackedClusters == null || queuedTrackedClusters.timestamp != queuedUntrackedClusters.timestamp))) {
continue;
}
Rect vehicleBox = DetermineVehicleEraseBox();
// load in the appropriate stuff to the occupancy grid
useOccupancyGrid = (Services.OccupancyGrid != null && !Services.OccupancyGrid.IsDisposed);
if (useOccupancyGrid) {
double occup_ts = Services.OccupancyGrid.LoadNewestGrid();
if (occup_ts < 0) {
useOccupancyGrid = false;
}
else {
double delta_ts = occup_ts - queuedUntrackedClusters.timestamp;
Services.Dataset.ItemAs<double>("occupancy delta ts").Add(delta_ts, queuedUntrackedClusters.timestamp);
}
}
occupancyDeletedCount = 0;
List<Obstacle> trackedObstacles;
if (queuedTrackedClusters == null) {
trackedObstacles = new List<Obstacle>();
}
else {
trackedObstacles = ProcessTrackedClusters(queuedTrackedClusters, vehicleBox);
}
List<Obstacle> untrackedObstacles = ProcessUntrackedClusters(queuedUntrackedClusters, trackedObstacles, vehicleBox);
List<Obstacle> finalObstacles = FinalizeProcessing(trackedObstacles, untrackedObstacles, queuedUntrackedClusters.timestamp);
processedObstacles = new ObstacleCollection(queuedUntrackedClusters.timestamp, finalObstacles);
Services.Dataset.ItemAs<int>("occupancy deleted count").Add(occupancyDeletedCount, queuedUntrackedClusters.timestamp);
queuedUntrackedClusters = null;
queuedTrackedClusters = null;
if (Services.DebuggingService.StepMode) {
Services.DebuggingService.SetCompleted(typeof(ObstaclePipeline));
}
Services.Dataset.MarkOperation("obstacle rate", LocalCarTimeProvider.LocalNow);
}
catch (Exception ex) {
OperationalTrace.WriteError("error processing obstacles: {0}", ex);
}
}
}
public ObstacleCollection GetProcessedObstacles(CarTimestamp timestamp, SAUDILevel saudi)
{
List <Obstacle> transformedObstacles = new List <Obstacle>();
if (Services.StateProvider.GetVehicleState().speed < 4.5)
{
Obstacle leftObstacle = GetSideObstacle(currentLeftSideObstacles);
Obstacle rightObstacle = GetSideObstacle(currentRightSideObstacles);
if (leftObstacle != null)
{
transformedObstacles.Add(leftObstacle);
}
if (rightObstacle != null)
{
transformedObstacles.Add(rightObstacle);
}
}
ObstacleCollection processedObstacles = this.processedObstacles;
if (processedObstacles != null)
{
if (transformedObstacles.Capacity < transformedObstacles.Count + processedObstacles.obstacles.Count)
{
transformedObstacles.Capacity = transformedObstacles.Count + processedObstacles.obstacles.Count;
}
RelativeTransform transform = Services.RelativePose.GetTransform(processedObstacles.timestamp, timestamp);
foreach (Obstacle obs in processedObstacles.obstacles)
{
if (!Settings.IgnoreTracks || obs.obstacleClass == ObstacleClass.StaticLarge || obs.obstacleClass == ObstacleClass.StaticSmall)
{
Obstacle newObs = obs.ShallowClone();
if (saudi == SAUDILevel.L1)
{
if (obs.obstacleClass == ObstacleClass.StaticSmall && Math.Abs(newObs.AvoidancePolygon.GetArea()) < L1_area_threshold)
{
continue;
}
}
else if (saudi == SAUDILevel.L2)
{
if (obs.obstacleClass == ObstacleClass.StaticSmall)
{
continue;
}
}
else if (saudi == SAUDILevel.L3)
{
if (obs.obstacleClass == ObstacleClass.StaticSmall || obs.obstacleClass == ObstacleClass.StaticLarge || obs.obstacleClass == ObstacleClass.DynamicNotCarlike || obs.obstacleClass == ObstacleClass.DynamicUnknown)
{
continue;
}
}
if (newObs.cspacePolygon != null)
{
newObs.cspacePolygon = newObs.cspacePolygon.Transform(transform);
}
if (newObs.extrudedPolygon != null)
{
newObs.extrudedPolygon = newObs.extrudedPolygon.Transform(transform);
}
if (newObs.obstaclePolygon != null)
{
newObs.obstaclePolygon = newObs.obstaclePolygon.Transform(transform);
}
if (newObs.predictedPolygon != null)
{
newObs.predictedPolygon = newObs.predictedPolygon.Transform(transform);
}
transformedObstacles.Add(newObs);
}
}
}
return(new ObstacleCollection(timestamp, transformedObstacles));
}
