public ChainedTrackingCommand(ITrackingCommand first, ITrackingCommand second)
 {
     this.first     = first;
     this.second    = second;
     this.label     = "<default>";
     firstCompleted = false;
 }
 public ChainedTrackingCommand(ITrackingCommand first, ITrackingCommand second)
 {
     this.first = first;
     this.second = second;
     this.label = "<default>";
     firstCompleted = false;
 }
Пример #3
0
 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);
        }
Пример #6
0
        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 QueueCommand(ITrackingCommand command)
 {
     if (!testMode) {
         // set the queue command
         queuedCommand = command;
     }
 }
Пример #11
0
        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;
 }
Пример #13
0
        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 TrackingCompletedEventArgs(CompletionResult result, object failureData, ITrackingCommand command)
 {
     this.result      = result;
     this.failureData = failureData;
     this.command     = command;
 }