public static bool AnyTimedOut(ref OperationalDataSource timeout)
{
if (forceTimeout) {
timeout = OperationalDataSource.TrackedClusters;
return true;
}
DateTime now = HighResDateTime.Now;
foreach (KeyValuePair<OperationalDataSource, TimeoutData> kvp in timeouts) {
if (kvp.Value.HasTimedOut(now)) {
timeout = kvp.Key;
return true;
}
}
return false;
}
public static bool AnyTimedOut(ref OperationalDataSource timeout)
{
if (forceTimeout)
{
timeout = OperationalDataSource.TrackedClusters;
return(true);
}
DateTime now = HighResDateTime.Now;
foreach (KeyValuePair <OperationalDataSource, TimeoutData> kvp in timeouts)
{
if (kvp.Value.HasTimedOut(now))
{
timeout = kvp.Key;
return(true);
}
}
return(false);
}
public static void MarkData(OperationalDataSource dataSource)
{
timeouts[dataSource].lastTime = HighResDateTime.Now;
}
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));
}
}
}
}
public static void MarkData(OperationalDataSource dataSource)
{
timeouts[dataSource].lastTime = HighResDateTime.Now;
}
