public void PushAbsolutePose(AbsolutePose pose) { lock (lockobj) { queue.Add(pose); if (!queue.VerifySort(delegate(AbsolutePose l, AbsolutePose r) { return(l.timestamp.CompareTo(r.timestamp)); })) { OperationalTrace.WriteError("absolute sort is donzoed, flushing queue"); queue.Clear(); } } }
private static void HandleResetStage2(bool initialize) { // if we're initializing, send the stop to both the scene estimator and local map // wait until we have confirmation that they're stopped, then restart them // wait until we get data // restart AI if (initialize) { forceTimeout = true; watchdogResetComplete = false; stage2Timer = null; watchdogResetEvent.Set(); OperationalTrace.WriteWarning("signaling restart to sensor fusion"); } else if (watchdogResetComplete) { stage2Timer = Stopwatch.StartNew(); watchdogResetComplete = false; OperationalTrace.WriteWarning("sensor fusion restart complete"); } else if (stage2Timer != null && stage2Timer.Elapsed > TimeSpan.FromSeconds(10)) { // possibly could wait until we have data to do this, but whatever OperationalTrace.WriteWarning("resetting arbiter"); // time to reset the AI stage2Timer.Reset(); try { ArbiterAdvancedRemote remote = GetRemote(); if (remote != null) { remote.Reset(); OperationalTrace.WriteWarning("arbiter reset complete"); } } catch (Exception ex) { OperationalTrace.WriteError("Error resetting AI in stage 2: {0}", ex); } } }
private void BehaviorProc() { Trace.CorrelationManager.StartLogicalOperation("Behavior Loop"); // set the default trace source for this thread OperationalTrace.ThreadTraceSource = TraceSource; OperationalDataSource previousTimeout = OperationalDataSource.None; using (MMWaitableTimer timer = new MMWaitableTimer((uint)(Settings.BehaviorPeriod * 1000))) { while (true) { if (Services.DebuggingService.StepMode) { Services.DebuggingService.WaitOnSequencer(typeof(BehaviorManager)); } else { // wait for the timer timer.WaitEvent.WaitOne(); } if (watchdogEvent != null) { try { watchdogEvent.Set(); } catch (Exception) { } } Services.Dataset.MarkOperation("planning rate", LocalCarTimeProvider.LocalNow); TraceSource.TraceEvent(TraceEventType.Verbose, 0, "starting behavior loop"); object param = null; IOperationalBehavior newBehavior = null; Behavior behaviorParam = null; OperationalDataSource timeoutSource = OperationalDataSource.Pose; bool forceHoldBrake = false; try { if (OperationalBuilder.BuildMode != BuildMode.Listen && TimeoutMonitor.HandleRecoveryState()) { forceHoldBrake = true; } } catch (Exception ex) { OperationalTrace.WriteError("error in recovery logic: {0}", ex); } if (OperationalBuilder.BuildMode == BuildMode.Realtime && TimeoutMonitor.AnyTimedOut(ref timeoutSource)) { if (timeoutSource != previousTimeout) { OperationalTrace.WriteError("data source {0} has timed out", timeoutSource); previousTimeout = timeoutSource; } // simply queue up a hold brake Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetHoldBrakeCommand(45)); } else { if (previousTimeout != OperationalDataSource.None) { OperationalTrace.WriteWarning("data source {0} is back online", previousTimeout); previousTimeout = OperationalDataSource.None; } if (forceHoldBrake || (Services.Operational != null && Services.Operational.GetCarMode() == CarMode.Human)) { queuedBehavior = null; queuedParam = null; queuedOrigBehavior = null; if (!(currentBehavior is HoldBrake)) { newBehavior = new HoldBrake(); param = null; queuedOrigBehavior = new HoldBrakeBehavior(); } } else { lock (queueLock) { // get the current queue param param = queuedParam; //TraceSource.TraceEvent(TraceEventType.Verbose, 0, "queued param: {0}", queuedParam == null ? "<null>" : queuedParam.ToString()); // chekc if there is a queued behavior newBehavior = queuedBehavior; queuedBehavior = null; behaviorParam = queuedOrigBehavior; queuedOrigBehavior = null; } } if (newBehavior != null) { // dispose of the old behavior if (currentBehavior != null && currentBehavior is IDisposable) { ((IDisposable)currentBehavior).Dispose(); } Trace.CorrelationManager.StartLogicalOperation("initialize"); TraceSource.TraceEvent(TraceEventType.Verbose, 8, "executing initialize on {0}", newBehavior.GetType().Name); // swap in the new behavior and initialize currentBehavior = newBehavior; try { currentBehavior.Initialize(behaviorParam); TraceSource.TraceEvent(TraceEventType.Verbose, 8, "initialize completed on {0}", currentBehavior.GetType().Name); } catch (Exception ex) { TraceSource.TraceEvent(TraceEventType.Warning, 4, "exception thrown when initializing behavior: {0}", ex); //throw; } finally { Trace.CorrelationManager.StopLogicalOperation(); } if (behaviorParam != null) { this.currentBehaviorType = behaviorParam.GetType(); } else { this.currentBehaviorType = null; } } else { //TraceSource.TraceEvent(TraceEventType.Verbose, 11, "queued behavior was null"); } // process the current behavior Trace.CorrelationManager.StartLogicalOperation("process"); try { if (currentBehavior != null) { Services.Dataset.ItemAs <string>("behavior string").Add(currentBehavior.GetName(), LocalCarTimeProvider.LocalNow); DateTime start = HighResDateTime.Now; currentBehavior.Process(param); TimeSpan diff = HighResDateTime.Now - start; lock (cycleTimeQueue) { cycleTimeQueue.Add(diff.TotalSeconds, LocalCarTimeProvider.LocalNow.ts); } } } catch (Exception ex) { // just ignore any exceptions for now // should log this somehow TraceSource.TraceEvent(TraceEventType.Warning, 5, "exception thrown when processing behavior: {0}", ex); } finally { Trace.CorrelationManager.StopLogicalOperation(); } } TraceSource.TraceEvent(TraceEventType.Verbose, 0, "ending behavior loop"); if (Services.DebuggingService.StepMode) { Services.DebuggingService.SetCompleted(typeof(BehaviorManager)); } } } }
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); } } }