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 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));
}
}
}
}
/// <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());
}));
}
}
}
}
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));
}
}
}
}
/// <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();
}
}
}
}
}
/// <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();
this.roadDisplay1.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);
}
}
}
