Exemplo n.º 1
0
        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));
                    }
                }
            }
        }