public override void ExecuteBehavior(UrbanChallenge.Behaviors.Behavior b)
{
if (displaySignals)
{
if (b.Decorators != null)
{
Console.Write("Decorators: ");
foreach (BehaviorDecorator bd in b.Decorators)
{
if (bd is TurnSignalDecorator)
{
TurnSignalDecorator tsd = (TurnSignalDecorator)bd;
Console.Write(tsd.Signal.ToString() + ", ");
}
else
{
Console.Write("Unrecognized, ");
}
}
Console.Write("\n");
}
else
{
Console.WriteLine("Decorators: null");
}
}
}
/// <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 HandleTurnSignal(TurnSignalDecorator decorator)
{
TraceSource.TraceEvent(TraceEventType.Verbose, 6, "turn signal on behavior: {0}", decorator.Signal);
Services.TrackingManager.SetTurnSignal(decorator.Signal);
}
private void HandleTurnSignal(TurnSignalDecorator decorator)
{
TraceSource.TraceEvent(TraceEventType.Verbose, 6, "turn signal on behavior: {0}", decorator.Signal);
Services.TrackingManager.SetTurnSignal(decorator.Signal);
}
