/// <summary>
/// Sets the ai mode
/// </summary>
/// <param name="mode"></param>
public override void SetAiMode(ArbiterMode mode)
{
try
{
ArbiterOutput.Output("Setting ai mode to ArbiterMode: " + mode.ToString());
switch (mode)
{
case ArbiterMode.Run:
this.intelligenceCore.RunIntelligence();
break;
case ArbiterMode.Pause:
this.intelligenceCore.PauseIntelligence();
break;
case ArbiterMode.Stop:
this.intelligenceCore.DestroyIntelligence();
break;
}
}
catch (Exception ex)
{
ArbiterOutput.Output("SetAiMode(ArbiterMode mode) Failed", ex);
}
}
/// <summary>
/// Shuts down the old ai thread
/// </summary>
public void DestroyIntelligence()
{
// notify
ArbiterOutput.Output("Attemptimg to Destroy Old Core Intelligence, Please Wait");
int count = 0;
// destroy old arbiter intelligence
while (this.CoreIntelligenceThread != null && this.CoreIntelligenceThread.IsAlive)
{
this.arbiterMode = ArbiterMode.Stop;
Thread.Sleep(100);
count++;
if (count > 50)
{
try
{
ArbiterOutput.Output("Cold not normally shutdown old intelligence, Aborting the core intelligence thread");
this.CoreIntelligenceThread.Abort();
}
catch (Exception)
{
}
// chill and wait for intelligence to exit
Thread.Sleep(1000);
}
}
// notify
ArbiterOutput.Output("No Intelligence Running, Destruction Successful");
}
/// <summary>
/// Jumpstarts the intelligence core
/// </summary>
public void Jumpstart()
{
// make sure to remove old
this.DestroyIntelligence();
// start new
ArbiterOutput.Output("Starting New Core Intelligence Thread");
this.arbiterMode = ArbiterMode.Run;
// start intelligence thread
CoreIntelligenceThread = new Thread(CoreIntelligence);
CoreIntelligenceThread.Priority = ThreadPriority.AboveNormal;
CoreIntelligenceThread.IsBackground = true;
CoreIntelligenceThread.Start();
// notify
ArbiterOutput.Output("Core Intelligence Thread Jumpstarted");
}
/// <summary>
/// Runs intelligence
/// </summary>
public void RunIntelligence()
{
ArbiterOutput.Output("Attempting to switch Core Intelligence to run mode");
if (this.CoreIntelligenceThread != null)
{
if (this.CoreIntelligenceThread.ThreadState != System.Threading.ThreadState.Running)
{
ArbiterOutput.Output("Core Intelligence set to run mode");
this.arbiterMode = ArbiterMode.Run;
}
else
{
ArbiterOutput.Output("Intelligence already running");
}
}
else
{
ArbiterOutput.Output("Intelligence Not Initialized");
}
}
public abstract void Restart(RndfNetwork rndf, Mdf mdf, ArbiterMode mode);
public abstract void Restart(RndfNetwork rndf, Mdf mdf, ArbiterMode mode);
public abstract void SetAiMode(ArbiterMode mode);
/// <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>
/// Pause intelligence
/// </summary>
public void PauseIntelligence()
{
ArbiterOutput.Output("Paused Intelligence");
this.arbiterMode = ArbiterMode.Pause;
}
/// <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>
/// Sets the ai mode
/// </summary>
/// <param name="mode"></param>
public override void SetAiMode(ArbiterMode mode)
{
try
{
ArbiterOutput.Output("Setting ai mode to ArbiterMode: " + mode.ToString());
switch (mode)
{
case ArbiterMode.Run:
this.intelligenceCore.RunIntelligence();
break;
case ArbiterMode.Pause:
this.intelligenceCore.PauseIntelligence();
break;
case ArbiterMode.Stop:
this.intelligenceCore.DestroyIntelligence();
break;
}
}
catch (Exception ex)
{
ArbiterOutput.Output("SetAiMode(ArbiterMode mode) Failed", ex);
}
}
/// <summary>
/// Runs intelligence
/// </summary>
public void RunIntelligence()
{
ArbiterOutput.Output("Attempting to switch Core Intelligence to run mode");
if (this.CoreIntelligenceThread != null)
{
if (this.CoreIntelligenceThread.ThreadState != System.Threading.ThreadState.Running)
{
ArbiterOutput.Output("Core Intelligence set to run mode");
this.arbiterMode = ArbiterMode.Run;
}
else
{
ArbiterOutput.Output("Intelligence already running");
}
}
else
{
ArbiterOutput.Output("Intelligence Not Initialized");
}
}
/// <summary>
/// Pause intelligence
/// </summary>
public void PauseIntelligence()
{
ArbiterOutput.Output("Paused Intelligence");
this.arbiterMode = ArbiterMode.Pause;
}
/// <summary>
/// Jumpstarts the intelligence core
/// </summary>
public void Jumpstart()
{
// make sure to remove old
this.DestroyIntelligence();
// start new
ArbiterOutput.Output("Starting New Core Intelligence Thread");
this.arbiterMode = ArbiterMode.Run;
// start intelligence thread
CoreIntelligenceThread = new Thread(CoreIntelligence);
CoreIntelligenceThread.Priority = ThreadPriority.AboveNormal;
CoreIntelligenceThread.IsBackground = true;
CoreIntelligenceThread.Start();
// notify
ArbiterOutput.Output("Core Intelligence Thread Jumpstarted");
}
/// <summary>
/// Shuts down the old ai thread
/// </summary>
public void DestroyIntelligence()
{
// notify
ArbiterOutput.Output("Attemptimg to Destroy Old Core Intelligence, Please Wait");
int count = 0;
// destroy old arbiter intelligence
while (this.CoreIntelligenceThread != null && this.CoreIntelligenceThread.IsAlive)
{
this.arbiterMode = ArbiterMode.Stop;
Thread.Sleep(100);
count++;
if (count > 50)
{
try
{
ArbiterOutput.Output("Cold not normally shutdown old intelligence, Aborting the core intelligence thread");
this.CoreIntelligenceThread.Abort();
}
catch (Exception)
{
}
// chill and wait for intelligence to exit
Thread.Sleep(1000);
}
}
// notify
ArbiterOutput.Output("No Intelligence Running, Destruction Successful");
}
public abstract void SetAiMode(ArbiterMode mode);
