public CompletionReport TestBehavior(Behavior b) { IOperationalBehavior operBehavior = null; try { testMode = true; Console.WriteLine("Testing behavior " + b.ToString() + " -- type " + b.GetType().Name); cachedCompletionReport = new SuccessCompletionReport(b.GetType()); HandleDecorators(b.Decorators); operBehavior = MapBehavior(b); operBehavior.Initialize(b); operBehavior.Process(null); Console.WriteLine("Result: " + cachedCompletionReport.ToString()); cachedCompletionReport.BehaviorId = b.UniqueId(); return(cachedCompletionReport); } catch (Exception ex) { TraceSource.TraceEvent(TraceEventType.Error, 0, "error testing behavior {0}: {1}", b.GetType(), ex); throw; } finally { if (operBehavior != null && operBehavior is IDisposable) { ((IDisposable)operBehavior).Dispose(); } } }
public void Execute(IOperationalBehavior b, object param, bool immediate) { lock (queueLock) { // if we want to immediately execute the next behavior, then we need to cancel the current one if (immediate && currentBehavior != null) { TraceSource.TraceEvent(TraceEventType.Verbose, 9, "in Execute, perfoming cancel on current behavior"); try { currentBehavior.Cancel(); } catch (Exception ex) { TraceSource.TraceEvent(TraceEventType.Error, 3, "cancelling the current behavior threw an exception: {0}", ex); throw; } } // queue up the new behavior queuedBehavior = b; queuedParam = param; queuedOrigBehavior = null; } }
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 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)); } } } }