/// <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); } }