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