public void ForceRedraw() { MMWaitableTimer timer = new MMWaitableTimer(100); while (true) { try { // timer wait timer.WaitEvent.WaitOne(); // invoke if (!this.IsDisposed) { this.BeginInvoke(new MethodInvoker(delegate() { this.Invalidate(); })); } } catch (Exception e) { Console.WriteLine(e.ToString()); } } }
private void SimProc() { using (MMWaitableTimer timer = new MMWaitableTimer(50)) { while (simRunning) { // wait on the timer timer.WaitEvent.WaitOne(); sumDt += 0.05; // call update on the dynamics vehicle this.DynamicsVehicle.Update(0.05, null); } } }
/// <summary> /// Update thread /// </summary> private void UpdateThread() { MMWaitableTimer timer = new MMWaitableTimer(100); while (updateThreadRun) { try { // timer wait timer.WaitEvent.WaitOne(); double speed = RemoraCommon.Communicator.GetVehicleSpeed() != null?RemoraCommon.Communicator.GetVehicleSpeed().Value : 0; // invoke if (!this.IsDisposed) { this.BeginInvoke(new MethodInvoker(delegate() { if (this.posteriorPoseDisplay != null && !this.posteriorPoseDisplay.IsDisposed) { this.posteriorPoseDisplay.UpdatePose(speed); } if (this.aiInformationWindow != null && !this.aiInformationWindow.IsDisposed) { this.aiInformationWindow.UpdateInformation(); } if (this.laneBrowser != null && !this.laneBrowser.IsDisposed) { this.laneBrowser.UpdateInformation(); } this.roadDisplay1.Invalidate(); })); } } catch (Exception e) { Console.WriteLine(e.ToString()); } } }
/// <summary> /// The simulation main thread /// </summary> public void Simulate() { // run sim at 10Hz MMWaitableTimer timer = new MMWaitableTimer((uint)this.settings.SimCycleTime); // notify this.simulationMain.SimulationModeLabel.Text = "Simulation Running"; this.simulationMain.SimulationModeLabel.Image = global::Simulator.Properties.Resources.Light_Bulb_On_16_n_p; // run while on while (this.SimulationState == SimulationState.Running) { if (stepMode) { // if we're in step mode, then wait on the step event or time out bool gotEvent = stepEvent.WaitOne(250, false); // if we timed out, start the loop over if (!gotEvent) { continue; } } else { // wait for 10hz timer.WaitEvent.WaitOne(); } try { // get world state WorldState ws = this.WorldService.GetWorldState(); lock (this.simulationMain.clientHandler) { // set world state to each client in the sim foreach (string client in this.simulationMain.clientHandler.VehicleToClientMap.Values) { try { // update client SimVehicleState nextState = this.simulationMain.clientHandler.AvailableClients[client].Update(ws, (double)this.settings.SimCycleTime / 1000.0); if (nextState != null) { // update state this.Vehicles[this.simulationMain.clientHandler.ClientToVehicleMap[client]].SimVehicleState = nextState; } else { throw new Exception("Received null SimVehicleState from " + client); } } catch (Exception e) { if (!this.simulationMain.IsDisposed) { this.simulationMain.BeginInvoke(new MethodInvoker(delegate() { // notify SimulatorOutput.WriteLine("Error Updating Client: " + client + ", Simulation Stopped"); // set state this.simulationMain.SimulationModeLabel.Text = "Simulation Stopped"; this.simulationMain.SimulationModeLabel.Image = global::Simulator.Properties.Resources.Light_Bulb_Off_16_n_p; // stop this.EndSimulation(); })); Console.WriteLine(e.ToString()); // leave break; } } } } // redraw this.simulationMain.BeginInvoke(new MethodInvoker(delegate() { // notify this.simulationMain.roadDisplay1.Invalidate(); })); } catch (Exception e) { if (!this.simulationMain.IsDisposed) { this.simulationMain.BeginInvoke(new MethodInvoker(delegate() { // notify SimulatorOutput.WriteLine("Error in outer sim loop:" + e.ToString()); })); } } } }
/// <summary> /// Manages excution of the test behaviros /// </summary> private void AsynchronousExecutionManager() { // execute ten times per second MMWaitableTimer mmwt = new MMWaitableTimer(100); // initialize this.testAsyncExecuteQueue = new Queue <Behavior>(); this.testBehaviorExecutionStopwatch = new Stopwatch(); this.testModeRecentCompleted = new List <TestBehaviorCompletion>(); this.testCurrentTest = null; this.testCompletionReport = null; // loop constantly while (true) { // make sre we are timing properly mmwt.WaitEvent.WaitOne(); // reset values this.testCurrentTest = null; this.testCompletionReport = null; // update the list of completed to only include those in last 10 seconds List <TestBehaviorCompletion> updatedRecent = new List <TestBehaviorCompletion>(); foreach (TestBehaviorCompletion tbc in this.testModeRecentCompleted) { if (this.GetVehicleState().Timestamp - tbc.CompletionTimestamp < 10.0) { updatedRecent.Add(tbc); } } this.testModeRecentCompleted = updatedRecent; try { if (!this.testComponentNeedsRoadNetwork && this.testComponentExists && testAsyncExecuteQueue.Count > 0) { // reset timer this.testBehaviorExecutionStopwatch.Stop(); this.testBehaviorExecutionStopwatch.Reset(); // eavior this.testCurrentTest = testAsyncExecuteQueue.Dequeue(); // thread the execution this.testBehaviorExecutionThread = new Thread(TestBehaviorExecutionThread); this.testBehaviorExecutionThread.IsBackground = true; this.testBehaviorExecutionStopwatch.Start(); this.testBehaviorExecutionThread.Start(); // test execution time while (this.testBehaviorExecutionThread.IsAlive) { // check time if (this.testBehaviorExecutionStopwatch.ElapsedMilliseconds / 1000.0 > 3.0) { try { ArbiterOutput.OutputNoLog("Test Behavior Execution took Longer than 3 seconds, aborting"); this.testBehaviorExecutionThread.Abort(); } catch (Exception) { } } // fine else { // take 10ms Thread.Sleep(100); } } // check completion report status if (this.testCompletionReport != null) { List <TestBehaviorCompletion> updated = new List <TestBehaviorCompletion>(this.testModeRecentCompleted.ToArray()); updated.Add(new TestBehaviorCompletion(this.GetVehicleState().Timestamp, this.testCompletionReport, this.testCurrentTest)); this.testModeRecentCompleted = updated; } this.testCompletionReport = null; this.testCurrentTest = null; } else { try { this.operationalTestFacade.Ping(); this.testComponentExists = true; } catch (Exception) { this.testComponentExists = false; this.testComponentNeedsRoadNetwork = true; this.TryOperationalTestFacadeConnect(); } if (!this.testComponentExists || this.testComponentNeedsRoadNetwork) { this.TryOperationalTestFacadeConnect(); } } } catch (Exception e) { } } }
/// <summary> /// Intelligence thread /// </summary> public void CoreIntelligence() { // wait for entry data this.WaitForEntryData(); // jumpstart behavioral this.Behavioral.Jumpstart(); // timer, run at 10Hz MMWaitableTimer cycleTimer = new MMWaitableTimer(100); // stopwatch Stopwatch stopwatch = new Stopwatch(); Stopwatch stopwatch2 = new Stopwatch(); // set initial state CoreCommon.CorePlanningState = new StartUpState(); // send the projection to the operational components CoreCommon.Communications.TrySendProjection(); CoreCommon.Communications.TryOperationalTestFacadeConnect(); // always run while (this.arbiterMode == ArbiterMode.Run || this.arbiterMode == ArbiterMode.Pause) { try { // make sure we run at 10Hz cycleTimer.WaitEvent.WaitOne(); if (this.arbiterMode == ArbiterMode.Run) { // start stopwatch stopwatch.Reset(); stopwatch.Start(); // reset ai information CoreCommon.CurrentInformation = new ArbiterInformation(); // check for null current state if (CoreCommon.CorePlanningState == null) { CoreCommon.CorePlanningState = new StartUpState(); throw new Exception("CoreCommon.CorePlanningState == null, returning to startup state"); } // get goal INavigableNode goal = StateReasoning.FilterGoal(CoreCommon.Communications.GetVehicleState()); // filter the state for needed changes CoreCommon.CorePlanningState = StateReasoning.FilterStates(CoreCommon.Communications.GetCarMode(), Behavioral); // set current state CoreCommon.CurrentInformation.CurrentState = CoreCommon.CorePlanningState.ShortDescription(); CoreCommon.CurrentInformation.CurrentStateInfo = CoreCommon.CorePlanningState.StateInformation(); // plan the maneuver Maneuver m = Behavioral.Plan( CoreCommon.Communications.GetVehicleState(), CoreCommon.Communications.GetVehicleSpeed().Value, CoreCommon.Communications.GetObservedVehicles(), CoreCommon.Communications.GetObservedObstacles(), CoreCommon.Communications.GetCarMode(), goal); // set next state CoreCommon.CorePlanningState = m.PrimaryState; CoreCommon.CurrentInformation.NextState = CoreCommon.CorePlanningState.ShortDescription(); CoreCommon.CurrentInformation.NextStateInfo = CoreCommon.CorePlanningState.StateInformation(); // get ignorable List <int> toIgnore = new List <int>(); if (m.PrimaryBehavior is StayInLaneBehavior) { StayInLaneBehavior silb = (StayInLaneBehavior)m.PrimaryBehavior; if (silb.IgnorableObstacles != null) { toIgnore.AddRange(silb.IgnorableObstacles); } else { ArbiterOutput.Output("stay in lane ignorable obstacles null"); } } else if (m.PrimaryBehavior is SupraLaneBehavior) { SupraLaneBehavior slb = (SupraLaneBehavior)m.PrimaryBehavior; if (slb.IgnorableObstacles != null) { toIgnore.AddRange(slb.IgnorableObstacles); } else { ArbiterOutput.Output("Supra lane ignorable obstacles null"); } } CoreCommon.CurrentInformation.FVTIgnorable = toIgnore.ToArray(); // reset the execution stopwatch this.executionStopwatch.Stop(); this.executionStopwatch.Reset(); this.executionStopwatch.Start(); // send behavior to communications CoreCommon.Communications.Execute(m.PrimaryBehavior); CoreCommon.CurrentInformation.NextBehavior = m.PrimaryBehavior.ToShortString(); CoreCommon.CurrentInformation.NextBehaviorInfo = m.PrimaryBehavior.ShortBehaviorInformation(); CoreCommon.CurrentInformation.NextSpeedCommand = m.PrimaryBehavior.SpeedCommandString(); CoreCommon.CurrentInformation.NextBehaviorTimestamp = m.PrimaryBehavior.TimeStamp.ToString("F6"); #region Turn Decorators // set turn signal decorators if (m.PrimaryBehavior.Decorators != null) { bool foundDec = false; foreach (BehaviorDecorator bd in m.PrimaryBehavior.Decorators) { if (bd is TurnSignalDecorator) { if (!foundDec) { TurnSignalDecorator tsd = (TurnSignalDecorator)bd; foundDec = true; CoreCommon.CurrentInformation.NextBehaviorTurnSignals = tsd.Signal.ToString(); } else { CoreCommon.CurrentInformation.NextBehaviorTurnSignals = "Multiple!"; } } } } #endregion // filter the lane state if (CoreCommon.CorePlanningState.UseLaneAgent) { this.coreLaneAgent.UpdateInternal(CoreCommon.CorePlanningState.InternalLaneState, CoreCommon.CorePlanningState.ResetLaneAgent); this.coreLaneAgent.UpdateEvidence(CoreCommon.Communications.GetVehicleState().Area); CoreCommon.CorePlanningState = this.coreLaneAgent.UpdateFilter(); } // log and send information to remote listeners CoreCommon.Communications.UpdateInformation(CoreCommon.CurrentInformation); // check cycle time stopwatch.Stop(); if (stopwatch.ElapsedMilliseconds > 100 || global::UrbanChallenge.Arbiter.Core.ArbiterSettings.Default.PrintCycleTimesAlways) { ArbiterOutput.Output("Cycle t: " + stopwatch.ElapsedMilliseconds.ToString()); } } } catch (Exception e) { // notify exception made its way up to the core thread ArbiterOutput.Output("\n\n"); ArbiterOutput.Output("Core Intelligence Thread caught exception!! \n"); ArbiterOutput.Output(" Exception type: " + e.GetType().ToString()); ArbiterOutput.Output(" Exception thrown by: " + e.TargetSite + "\n"); ArbiterOutput.Output(" Stack Trace: " + e.StackTrace + "\n"); if (e is NullReferenceException) { NullReferenceException nre = (NullReferenceException)e; ArbiterOutput.Output("Null reference exception from: " + nre.Source); } ArbiterOutput.Output("\n"); if (this.executionStopwatch.ElapsedMilliseconds / 1000.0 > 3.0) { ArbiterOutput.Output(" Time since last execution more then 3 seconds"); ArbiterOutput.Output(" Resetting and Restarting Intelligence"); try { Thread tmp = new Thread(ResetThread); tmp.IsBackground = true; this.arbiterMode = ArbiterMode.Stop; tmp.Start(); } catch (Exception ex) { ArbiterOutput.Output("\n\n"); ArbiterOutput.Output("Core Intelligence Thread caught exception attempting to restart itself1!!!!HOLYCRAP!! \n"); ArbiterOutput.Output(" Exception thrown by: " + ex.TargetSite + "\n"); ArbiterOutput.Output(" Stack Trace: " + ex.StackTrace + "\n"); ArbiterOutput.Output(" Resetting planning state to startup"); ArbiterOutput.Output("\n"); CoreCommon.CorePlanningState = new StartUpState(); } } } } }
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 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)); } } } }