/// <summary>
/// Output error to console as well as log file
/// </summary>
/// <param name="s">String to output</param>
/// <param name="e">Exception to output</param>
public static void Output(string s, Exception e)
{
s = "\n" + s + ": \n" + e.ToString() + "\n\n";
try
{
// write to log
ArbiterOutput.WriteToLog(DateTime.Now.ToString() + ": " + s);
}
catch (Exception ex)
{
Console.WriteLine("Error writing to log: \n" + ex.ToString());
}
// write to console
Console.WriteLine(s);
try
{
if (CoreCommon.Communications != null)
{
// output
CoreCommon.Communications.SendOutput(s);
}
}
catch (Exception ex)
{
Console.WriteLine("Error outputting to arbiter message channel: \n" + ex.ToString());
}
}
/// <summary>
/// Operational competes a behavior
/// </summary>
/// <param name="report"></param>
public override void OnCompletionReport(CompletionReport report)
{
try
{
if (report.Result == CompletionResult.Success)
{
ArbiterOutput.OutputNoLog("Received Completion Report: " + report.BehaviorType.ToString() + ", Result: " + report.Result.ToString());
}
else if (this.ArbiterBlockageHandler != null && report is TrajectoryBlockedReport)
{
this.ArbiterBlockageHandler.OnBlockageReport((TrajectoryBlockedReport)report);
}
lock (this.recentReports)
{
this.recentReports = new List <KeyValuePair <CompletionReport, DateTime> >(this.recentReports.ToArray());
this.recentReports.Add(new KeyValuePair <CompletionReport, DateTime>(report, DateTime.Now));
}
}
catch (Exception e)
{
try
{
ArbiterOutput.OutputNoLog("Error in completion report");
ArbiterOutput.WriteToLog(e.ToString());
}
catch (Exception)
{
}
}
}
/// <summary>
/// Test executue a behavior
/// </summary>
/// <param name="b"></param>
/// <param name="completionReport"></param>
public bool TestExecute(Behavior b, out CompletionReport completionReport)
{
if (this.operationalTestFacade != null)
{
try
{
b.TimeStamp = CoreCommon.Communications.GetVehicleState().Timestamp;
completionReport = this.operationalTestFacade.TestExecuteBehavior(b);
ArbiterOutput.OutputNoLog("test execute: " + b.ShortBehaviorInformation() + " completion report: " + completionReport.GetType().ToString());
return(completionReport.Result == CompletionResult.Success);
}
catch (Exception e)
{
completionReport = new SuccessCompletionReport(b.GetType());
ArbiterOutput.OutputNoLog("test execute: " + b.ShortBehaviorInformation() + " encountered error: " + e.ToString());
this.TryOperationalTestFacadeConnect();
return(true);
}
}
else
{
completionReport = new SuccessCompletionReport(b.GetType());
ArbiterOutput.OutputNoLog("test execute: " + b.ShortBehaviorInformation() + " encountered error: operational test facade does not exist");
this.TryOperationalTestFacadeConnect();
return(true);
}
}
/// <summary>
/// Sends road network projection to the operational layer
/// </summary>
public void SendProjection()
{
try
{
if (CoreCommon.RoadNetwork != null)
{
if (this.operationalFacade != null)
{
this.testComponentNeedsRoadNetwork = true;
this.operationalFacade.SetRoadNetwork(CoreCommon.RoadNetwork);
}
else
{
ArbiterOutput.Output("Need to connect to operational before can send road network");
}
}
else
{
ArbiterOutput.Output("Road network cannot be null to send to operational");
}
}
catch (Exception e)
{
ArbiterOutput.Output("Error setting road network in operational, sending projection");
this.operationalFacade.SetProjection(CoreCommon.RoadNetwork.PlanarProjection);
Console.WriteLine(e.ToString());
}
}
public void Execute(Behavior behavior)
{
try
{
if (!(behavior is NullBehavior))
{
Stopwatch bsw = new Stopwatch();
bsw.Reset();
bsw.Start();
// execute operational behavior
operationalFacade.ExecuteBehavior(behavior);
bsw.Stop();
if (bsw.ElapsedMilliseconds > 50)
{
ArbiterOutput.Output("Delay: Operational Execution Time: " + bsw.ElapsedMilliseconds.ToString());
}
}
}
catch (Exception e)
{
// Notify
ArbiterOutput.Output("Errror executing operational behavior: " + e.ToString());
}
}
/// <summary>
/// Clear all old completion reports
/// </summary>
public void ClearCompletionReports()
{
try
{
this.recentReports = new List <KeyValuePair <CompletionReport, DateTime> >();
ArbiterOutput.Output("Cleared Completion Reports");
}
catch (Exception) { }
}
/// <summary>
/// Called when message sent to us
/// </summary>
/// <param name="channelName"></param>
/// <param name="message"></param>
public void MessageArrived(string channelName, object message)
{
try
{
if (channelName == "ArbiterSceneEstimatorPositionChannel" + this.remotingSuffix &&
message is VehicleState)
{
// cast and set
vehicleState = (VehicleState)message;
}
else if (channelName == "ObservedObstacleChannel" + this.remotingSuffix &&
message is SceneEstimatorUntrackedClusterCollection)
{
// cast and set
observedObstacles = (SceneEstimatorUntrackedClusterCollection)message;
}
else if (channelName == "ObservedVehicleChannel" + this.remotingSuffix &&
message is SceneEstimatorTrackedClusterCollection)
{
// check if not ignoring vehicles
if (!Arbiter.Core.ArbiterSettings.Default.IgnoreVehicles)
{
// cast and set
observedVehicles = (SceneEstimatorTrackedClusterCollection)message;
}
}
else if (channelName == "VehicleSpeedChannel" + this.remotingSuffix &&
message is double)
{
// cast and set
vehicleSpeed = (double)message;
}
else if (channelName == "SideObstacleChannel" + this.remotingSuffix &&
message is SideObstacles)
{
SideObstacles sideSickObstacles = (SideObstacles)message;
if (sideSickObstacles.side == SideObstacleSide.Driver)
{
this.sideSickObstaclesDriver = sideSickObstacles;
}
else
{
this.sideSickObstaclesPass = sideSickObstacles;
}
}
}
catch (Exception ex)
{
ArbiterOutput.Output("Error receiving message: " + ex.ToString());
}
}
public void UpdateInformation(ArbiterInformation arbiterInformation)
{
if (arbiterInformation != null)
{
//Stopwatch stopwatch3 = new Stopwatch();
//stopwatch3.Reset();
//stopwatch3.Start();
ArbiterOutput.WriteToLog(arbiterInformation.LogString());
//stopwatch3.Stop();
//Console.WriteLine("SW 3: " + stopwatch3.ElapsedMilliseconds.ToString());
//stopwatch3.Reset();
//stopwatch3.Start();
this.currentInformation = arbiterInformation;
//stopwatch3.Stop();
//Console.WriteLine("SW 4: " + stopwatch3.ElapsedMilliseconds.ToString());
}
}
/// <summary>
/// Try to connect to the operational facade
/// </summary>
public void TryOperationalTestFacadeConnect()
{
if (global::UrbanChallenge.Arbiter.Core.ArbiterSettings.Default.UseTestOperational)
{
try
{
// check if test facade ok
bool testFacadeGood = this.operationalTestFacade != null;
if (this.operationalTestFacade != null)
{
try
{
this.operationalTestFacade.Ping();
this.testComponentExists = true;
}
catch (Exception)
{
testFacadeGood = false;
}
}
// set the operational test facade if not ok
if (!testFacadeGood)
{
// get the operational layer
this.operationalTestFacade = (OperationalTestComponentFacade)objectDirectory.Resolve("OperationalTestComponentService" + this.RemotingSuffix);
this.testComponentExists = true;
}
// try to send road network
if (CoreCommon.RoadNetwork != null)
{
this.operationalTestFacade.SetRoadNetwork(CoreCommon.RoadNetwork);
this.testComponentNeedsRoadNetwork = false;
}
}
catch (Exception ex)
{
ArbiterOutput.OutputNoLog("Error registering with operational test service: " + ex.ToString());
}
}
}
/// <summary>
/// Sends road network projection to the operational layer
/// </summary>
public void TrySendProjection()
{
ArbiterOutput.Output("Attempting to set Operational Road Network");
if (CoreCommon.RoadNetwork != null)
{
if (this.operationalFacade != null)
{
try
{
this.operationalFacade.SetRoadNetwork(CoreCommon.RoadNetwork);
}
catch (Exception ex)
{
ArbiterOutput.Output("Error setting road network in operational, sending projection");
Console.WriteLine(ex.ToString());
try
{
this.operationalFacade.SetProjection(CoreCommon.RoadNetwork.PlanarProjection);
}
catch (Exception e)
{
Console.WriteLine("error setting projection in op");
}
}
}
else
{
ArbiterOutput.Output("Need to connect to operational before can send road network");
}
}
else
{
ArbiterOutput.Output("Road network cannot be null to send to operational");
}
ArbiterOutput.Output("Leaving TrySendOperationalProjection");
}
/// <summary>
/// Shuts down the communicator and unsubscribes from channels, whatnot
/// </summary>
public void Shutdown()
{
try
{
if (vehicleStateChannel != null)
{
// unsubscribe from channels
vehicleStateChannel.Unsubscribe(vehicleStateChannelToken);
observedObstacleChannel.Unsubscribe(observedObstacleChannelToken);
observedVehicleChannel.Unsubscribe(observedVehicleChannelToken);
vehicleSpeedChannel.Unsubscribe(vehicleSpeedChannelToken);
sideObstacleChannel.Unsubscribe(sideObstacleChannelToken);
}
// notify
ArbiterOutput.Output("Unsubscribed from channels");
}
catch (Exception e)
{
// notify
ArbiterOutput.Output("Error in shutting down registered channels");
ArbiterOutput.WriteToLog(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>
/// Try to reconnect to stuff
/// </summary>
public void TryReconnect()
{
this.CommunicationsReady = false;
ArbiterOutput.Output("Set comms ready flag to false");
}
/// <summary>
/// Keeps watch over communications components and determines if any need to be restarted
/// </summary>
public void Watchdog()
{
// always loop
while (true)
{
try
{
if (Arbiter.Core.ArbiterSettings.Default.IgnoreVehicles)
{
ArbiterOutput.Output("Ignoring Tracked Clusters");
}
#region Update Completion reports with 3 Sec Timeout
lock (this.recentReports)
{
List <KeyValuePair <CompletionReport, DateTime> > crs = new List <KeyValuePair <CompletionReport, DateTime> >();
foreach (KeyValuePair <CompletionReport, DateTime> cr in this.recentReports)
{
TimeSpan diff = DateTime.Now.Subtract(cr.Value);
ArbiterOutput.WriteToLog("Comm line 263 test diff.ToString: " + diff.ToString());
if (diff.Seconds < 2 && cr.Value.Date.Equals(DateTime.Now.Date))
{
crs.Add(cr);
}
else
{
ArbiterOutput.WriteToLog("Removed Completion Report: " + cr.Key.BehaviorType.ToString() + ", " + cr.Key.Result.ToString());
}
}
this.recentReports = crs;
}
#endregion
#region Check for communications readiness
if (!this.CommunicationsReady)
{
try
{
// configure
this.Configure();
}
catch (Exception e)
{
// notify
ArbiterOutput.Output("Error in communications watchdog, configuration");
ArbiterOutput.WriteToLog(e.ToString());
}
}
if (!this.CommunicationsReady)
{
try
{
// make sure nothing else registered
this.Shutdown();
// register services
this.Register();
}
catch (Exception e)
{
// notify
ArbiterOutput.Output("Error in communications watchdog, registration");
ArbiterOutput.WriteToLog(e.ToString());
}
}
#endregion
#region Get Car Mode (Acts as Operational Ping and know then Comms Ready)
try
{
if (this.operationalFacade != null)
{
// update the car mode
this.carMode = this.operationalFacade.GetCarMode();
// set comms ready as true given success
if (!this.CommunicationsReady)
{
this.CommunicationsReady = true;
}
}
else
{
this.CommunicationsReady = false;
}
}
catch (Exception e)
{
// notify
ArbiterOutput.Output("Error retreiving car mode from operational in watchdog, attempting to reconnect");
// log
ArbiterOutput.WriteToLog(e.ToString());
// set comms ready as false
this.CommunicationsReady = false;
}
#endregion
}
catch (Exception e)
{
ArbiterOutput.Output("Error in communications watchdog", e);
}
// wait for cycle time
Thread.Sleep(1000);
}
}
