public ChainedTrackingCommand(ITrackingCommand first, ITrackingCommand second) { this.first = first; this.second = second; this.label = "<default>"; firstCompleted = false; }
public void QueueCommand(ITrackingCommand command) { if (!testMode) { // set the queue command queuedCommand = command; } }
protected override void OnSetup(out ITrackingCommand sut) { action = p => executeCallCount++; canExecute = p => ++ canExecuteCallCount > 0; sut = new TrackingCommand <Parameter>( action, EventName, ScreenName, canExecute, analytics: analyticsMock.Object); }
protected override void OnSetup(out ITrackingCommand sut) { func = () => { executeCallCount++; return(Task.CompletedTask); }; canExecute = () => ++ canExecuteCallCount > 0; sut = new AsyncTrackingCommand( func, EventName, ScreenName, canExecute, analytics: analyticsMock.Object); }
public TrackingManager(ICommandTransport commandTransport, bool testMode) { // check that the command transport is legit if (commandTransport == null) { throw new ArgumentNullException("commandTransport"); } this.testMode = testMode; // create the command queue deltaSteeringQueue = new TimeWindowQueue <double>(3); // store the command transport this.commandTransport = commandTransport; // set up a null tracking command initially currentCommand = new NullTrackingCommand(); // mark the current completion result as "working" currentResult = CompletionResult.Working; if (!testMode) { AsyncFlowControl?flowControl = null; if (!ExecutionContext.IsFlowSuppressed()) { flowControl = ExecutionContext.SuppressFlow(); } // set up the thread trackingThread = new Thread(TrackingProc); // set it as a background thread trackingThread.IsBackground = true; // set it as higher priority trackingThread.Priority = ThreadPriority.AboveNormal; // give it a useful name trackingThread.Name = "Tracking Thread"; // start her up trackingThread.Start(); if (flowControl != null) { flowControl.Value.Undo(); } } }
public TrackingManager(ICommandTransport commandTransport, bool testMode) { // check that the command transport is legit if (commandTransport == null) { throw new ArgumentNullException("commandTransport"); } this.testMode = testMode; // create the command queue deltaSteeringQueue = new TimeWindowQueue<double>(3); // store the command transport this.commandTransport = commandTransport; // set up a null tracking command initially currentCommand = new NullTrackingCommand(); // mark the current completion result as "working" currentResult = CompletionResult.Working; if (!testMode) { AsyncFlowControl? flowControl = null; if (!ExecutionContext.IsFlowSuppressed()) { flowControl = ExecutionContext.SuppressFlow(); } // set up the thread trackingThread = new Thread(TrackingProc); // set it as a background thread trackingThread.IsBackground = true; // set it as higher priority trackingThread.Priority = ThreadPriority.AboveNormal; // give it a useful name trackingThread.Name = "Tracking Thread"; // start her up trackingThread.Start(); if (flowControl != null) { flowControl.Value.Undo(); } } }
protected abstract void OnSetup(out ITrackingCommand sut);
private void TrackingProc() { Trace.CorrelationManager.StartLogicalOperation("tracking loop"); OperationalTrace.ThreadTraceSource = TraceSource; double? lastSteeringCommand = null; using (MMWaitableTimer timer = new MMWaitableTimer((uint)(Settings.TrackingPeriod*1000))) { // loop infinitely while (true) { if (Services.DebuggingService.StepMode) { Services.DebuggingService.WaitOnSequencer(typeof(TrackingManager)); } else { // wait for the timer timer.WaitEvent.WaitOne(); } Services.Dataset.MarkOperation("tracking rate", LocalCarTimeProvider.LocalNow); // check if there's a new command to switch in ITrackingCommand newCommand = Interlocked.Exchange(ref queuedCommand, null); if (newCommand != null) { TraceSource.TraceEvent(TraceEventType.Verbose, 0, "received new command: {0}", newCommand); // set the current result to working currentResult = CompletionResult.Working; // set the current command to the new command currentCommand = newCommand; } // check if we've completed or had a failure if (currentResult == CompletionResult.Working) { TrackingData td = new TrackingData(null, null, null, null, CompletionResult.Failed); try { // get the tracking data Trace.CorrelationManager.StartLogicalOperation("process"); td = currentCommand.Process(); } catch (Exception ex) { // log this somehow // if we get here, the completion result retains the failed label, so we won't output anything TraceSource.TraceEvent(TraceEventType.Error, 1, "exception tracking command: {0}", ex); } finally { Trace.CorrelationManager.StopLogicalOperation(); } // check if there is a failure if (td.result == CompletionResult.Failed) { td = new TrackingData(null, null, null, null, CompletionResult.Failed); } // output the command try { Trace.CorrelationManager.StartLogicalOperation("command output"); commandTransport.SetCommand(td.engineTorque, td.brakePressure, td.steeringAngle, td.gear); } catch (Exception ex) { TraceSource.TraceEvent(TraceEventType.Error, 0, "error setting command on transport: {0}", ex); } finally { Trace.CorrelationManager.StopLogicalOperation(); } // queue the command lock (deltaSteeringQueue) { if (lastSteeringCommand != null && td.steeringAngle != null) { double delta = td.steeringAngle.Value - lastSteeringCommand.Value; deltaSteeringQueue.Add(delta, LocalCarTimeProvider.LocalNow.ts); } Services.Dataset.ItemAs<double>("delta steering sigma").Add(DeltaSteeringStdDev, LocalCarTimeProvider.LocalNow); lastSteeringCommand = td.steeringAngle; } if (td.brakePressure.HasValue && td.brakePressure.Value > 90) { td.brakePressure = 90; } // output the tracking commands to the UI CarTimestamp now = Services.RelativePose.CurrentTimestamp; if (td.steeringAngle.HasValue) { Services.Dataset.ItemAs<double>("commanded steering").Add(td.steeringAngle.Value, now); } if (td.engineTorque.HasValue) { Services.Dataset.ItemAs<double>("commanded engine torque").Add(td.engineTorque.Value, now); } if (td.brakePressure.HasValue) { Services.Dataset.ItemAs<double>("commanded brake pressure").Add(td.brakePressure.Value, now); } if (td.result != CompletionResult.Working && trackingCompleted != null) { Trace.CorrelationManager.StartLogicalOperation("tracking completed"); TraceSource.TraceEvent(TraceEventType.Information, 0, "tracking completed, invoking event, result {0}", td.result); // raise the completed event asynchronously so we can go on processing try { trackingCompleted.BeginInvoke(this, new TrackingCompletedEventArgs(td.result, null, currentCommand), OnTrackingCompletedFinished, trackingCompleted); } catch (Exception ex) { TraceSource.TraceEvent(TraceEventType.Error, 0, "exception thrown in beging invoke of tracking complete event: {0}", ex); } finally { Trace.CorrelationManager.StopLogicalOperation(); } } // update the current result currentResult = td.result; } // flush the command transport every iteration commandTransport.Flush(); if (Services.DebuggingService.StepMode) { Services.DebuggingService.SetCompleted(typeof(TrackingManager)); } } } }
public void Process(object param) { if (cancelled) { return; } if (!passCompleted && !checkMode) { return; } if (param != null && param is UTurnBehavior) { HandleBehavior((UTurnBehavior)param); } ITrackingCommand trackingCommand = null; if (pass == UTurnPass.Initializing) { pass = CheckInitialAlignment(); } else if (CheckFinalExit()) { pass = UTurnPass.Final; } // determine which pass we just finished if (pass == UTurnPass.Backward) { if (passCompleted) { // build the forward pass and run it bool finalPass; trackingCommand = BuildForwardPass(out finalPass); if (finalPass) { pass = UTurnPass.Final; } else { pass = UTurnPass.Forward; } passCompleted = false; } else if (checkMode) { CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp; double remainingDist = GetRemainingStopDistance(curTimestamp); double collisionDist = GetObstacleCollisionDistance(false, curvature, remainingDist, GetObstacles(curTimestamp)); if (collisionDist < remainingDist && collisionDist < 3) { Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetHoldBrakeCommand()); passCompleted = true; } } } else if (pass == UTurnPass.Forward) { if (passCompleted) { trackingCommand = BuildBackwardPass(); pass = UTurnPass.Backward; passCompleted = false; } else if (checkMode) { CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp; double remainingDist = GetRemainingStopDistance(curTimestamp); double collisionDist = GetObstacleCollisionDistance(true, curvature, remainingDist, GetObstacles(curTimestamp)); if (collisionDist < remainingDist && collisionDist < 3) { Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetHoldBrakeCommand()); passCompleted = true; } } } else if (pass == UTurnPass.Final) { // execute a stay in lane behavior Services.BehaviorManager.Execute(new StayInLaneBehavior(finalLane, finalSpeedCommand, null), null, false); passCompleted = false; Services.BehaviorManager.ForwardCompletionReport(new SuccessCompletionReport(typeof(UTurnBehavior))); } if (trackingCommand != null) { Services.TrackingManager.QueueCommand(trackingCommand); } }
public TrackingCompletedEventArgs(CompletionResult result, object failureData, ITrackingCommand command) { this.result = result; this.failureData = failureData; this.command = command; }
private void TrackingProc() { Trace.CorrelationManager.StartLogicalOperation("tracking loop"); OperationalTrace.ThreadTraceSource = TraceSource; double?lastSteeringCommand = null; using (MMWaitableTimer timer = new MMWaitableTimer((uint)(Settings.TrackingPeriod * 1000))) { // loop infinitely while (true) { if (Services.DebuggingService.StepMode) { Services.DebuggingService.WaitOnSequencer(typeof(TrackingManager)); } else { // wait for the timer timer.WaitEvent.WaitOne(); } Services.Dataset.MarkOperation("tracking rate", LocalCarTimeProvider.LocalNow); // check if there's a new command to switch in ITrackingCommand newCommand = Interlocked.Exchange(ref queuedCommand, null); if (newCommand != null) { TraceSource.TraceEvent(TraceEventType.Verbose, 0, "received new command: {0}", newCommand); // set the current result to working currentResult = CompletionResult.Working; // set the current command to the new command currentCommand = newCommand; } // check if we've completed or had a failure if (currentResult == CompletionResult.Working) { TrackingData td = new TrackingData(null, null, null, null, CompletionResult.Failed); try { // get the tracking data Trace.CorrelationManager.StartLogicalOperation("process"); td = currentCommand.Process(); } catch (Exception ex) { // log this somehow // if we get here, the completion result retains the failed label, so we won't output anything TraceSource.TraceEvent(TraceEventType.Error, 1, "exception tracking command: {0}", ex); } finally { Trace.CorrelationManager.StopLogicalOperation(); } // check if there is a failure if (td.result == CompletionResult.Failed) { td = new TrackingData(null, null, null, null, CompletionResult.Failed); } // output the command try { Trace.CorrelationManager.StartLogicalOperation("command output"); commandTransport.SetCommand(td.engineTorque, td.brakePressure, td.steeringAngle, td.gear); } catch (Exception ex) { TraceSource.TraceEvent(TraceEventType.Error, 0, "error setting command on transport: {0}", ex); } finally { Trace.CorrelationManager.StopLogicalOperation(); } // queue the command lock (deltaSteeringQueue) { if (lastSteeringCommand != null && td.steeringAngle != null) { double delta = td.steeringAngle.Value - lastSteeringCommand.Value; deltaSteeringQueue.Add(delta, LocalCarTimeProvider.LocalNow.ts); } Services.Dataset.ItemAs <double>("delta steering sigma").Add(DeltaSteeringStdDev, LocalCarTimeProvider.LocalNow); lastSteeringCommand = td.steeringAngle; } if (td.brakePressure.HasValue && td.brakePressure.Value > 90) { td.brakePressure = 90; } // output the tracking commands to the UI CarTimestamp now = Services.RelativePose.CurrentTimestamp; if (td.steeringAngle.HasValue) { Services.Dataset.ItemAs <double>("commanded steering").Add(td.steeringAngle.Value, now); } if (td.engineTorque.HasValue) { Services.Dataset.ItemAs <double>("commanded engine torque").Add(td.engineTorque.Value, now); } if (td.brakePressure.HasValue) { Services.Dataset.ItemAs <double>("commanded brake pressure").Add(td.brakePressure.Value, now); } if (td.result != CompletionResult.Working && trackingCompleted != null) { Trace.CorrelationManager.StartLogicalOperation("tracking completed"); TraceSource.TraceEvent(TraceEventType.Information, 0, "tracking completed, invoking event, result {0}", td.result); // raise the completed event asynchronously so we can go on processing try { trackingCompleted.BeginInvoke(this, new TrackingCompletedEventArgs(td.result, null, currentCommand), OnTrackingCompletedFinished, trackingCompleted); } catch (Exception ex) { TraceSource.TraceEvent(TraceEventType.Error, 0, "exception thrown in beging invoke of tracking complete event: {0}", ex); } finally { Trace.CorrelationManager.StopLogicalOperation(); } } // update the current result currentResult = td.result; } // flush the command transport every iteration commandTransport.Flush(); if (Services.DebuggingService.StepMode) { Services.DebuggingService.SetCompleted(typeof(TrackingManager)); } } } }