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