public AbsolutePose GetAbsolutePose(CarTimestamp timestamp) { lock (lockobj) { //OperationalTrace.WriteVerbose("tride to find absolute pose for {0}", timestamp); //return queue.FindClosest(delegate(AbsolutePose p) { return p.timestamp.CompareTo(timestamp); }); int i = queue.Count - 1; while (i >= 0 && queue[i].timestamp > timestamp) { i--; } CarTimestamp ts = CarTimestamp.Invalid; if (i >= 0 && i < queue.Count) { ts = queue[i].timestamp; } OperationalTrace.WriteVerbose("tried to find absolute pose for {0}, found index {1} timestamp {2}", timestamp, i, ts); if (i < 0) { throw new TransformationNotFoundException(timestamp); } return(queue[i]); } }
static void Main(string[] args) { foreach (string arg in args) { string lowerArg = arg.ToLower(); if (lowerArg == "/test" || lowerArg == "/t" || lowerArg == "-test" || lowerArg == "-t") { Settings.TestMode = true; } } AppDomain.CurrentDomain.UnhandledException += new UnhandledExceptionEventHandler(CurrentDomain_UnhandledException); AsyncFlowControl flowControl = ExecutionContext.SuppressFlow(); OperationalTrace.WriteInformation("starting up operational"); Console.SetWindowSize(Math.Min(130, Console.LargestWindowWidth), Math.Min(50, Console.LargestWindowHeight)); Console.SetBufferSize(Math.Min(130, Console.LargestWindowWidth), 999); OperationalBuilder.Build(); Console.Write("operational> "); flowControl.Undo(); while (Console.ReadLine() != "quit") { } }
private static void HandleResetStage1(bool initialize) { if (initialize) { stage1Timer = Stopwatch.StartNew(); OperationalTrace.WriteWarning("forcing car mode to human"); // fake transition to human mode if (Services.Operational != null) { Services.Operational.ForceCarMode(CarMode.Human); } } else if (stage1Timer.Elapsed > TimeSpan.FromSeconds(2)) { stage1Timer.Reset(); OperationalTrace.WriteWarning("clearing forced car mode"); // switch to an unknown car mode if (Services.Operational != null) { Services.Operational.ForceCarMode(CarMode.Unknown); } } }
public void SetCommand(double?engineTorque, double?brakePressue, double?steering, TransmissionGear?gear) { if (Settings.TestMode) { return; } if (steering.HasValue && Math.Abs(steering.Value) > TahoeParams.SW_max) { OperationalTrace.WriteWarning("steering out of range: received {0}, limit {1}", steering.Value, TahoeParams.SW_max); } //Console.WriteLine("command--steering: {0}, torque: {1}, brake: {2}, gear: {3}", steering, engineTorque, brakePressue, gear); try { dynamicsSim.SetSteeringBrakeThrottle(steering, engineTorque, brakePressue); if (gear.HasValue) { dynamicsSim.SetTransmissionGear(gear.Value); } } catch (SocketException) { // get the DynamicsSimFacade dynamicsSim = (DynamicsSimFacade)CommBuilder.GetObject(DynamicsSimFacade.ServiceName); throw; } }
public LinePath LinearizeCenterLine(LinearizationOptions options) { LinePath transformedPath = centerlinePath; if (options.Timestamp.IsValid) { RelativeTransform relTransform = Services.RelativePose.GetTransform(timestamp, options.Timestamp); OperationalTrace.WriteVerbose("in LinearizeCenterLine, tried to find {0}->{1}, got {2}->{3}", timestamp, options.Timestamp, relTransform.OriginTimestamp, relTransform.EndTimestamp); transformedPath = centerlinePath.Transform(relTransform); } LinePath.PointOnPath startPoint = transformedPath.AdvancePoint(centerlinePath.ZeroPoint, options.StartDistance); LinePath subPath = new LinePath();; if (options.EndDistance > options.StartDistance) { subPath = transformedPath.SubPath(startPoint, options.EndDistance - options.StartDistance); } if (subPath.Count < 2) { subPath.Clear(); Coordinates startPt = startPoint.Location; subPath.Add(startPt); subPath.Add(centerlinePath.GetSegment(startPoint.Index).UnitVector *Math.Max(options.EndDistance - options.StartDistance, 0.1) + startPt); } return(subPath); }
private void ReadVersion1Packet(MemoryStream ms) { BinaryReader reader = new BinaryReader(ms); // get the sequence number int seqenceNumber = reader.ReadInt32(); // get the message type byte messageType = reader.ReadByte(); // get if we're compressed or not byte compressed = reader.ReadByte(); Stream sourceStream = ms; if (compressed == packet_compressed) { // make a decompression stream sourceStream = new DeflateStream(ms, CompressionMode.Decompress); } // deserialize the object if (messageType == packet_type_arbiter_command) { try { Behavior b = (Behavior)behaviorFormatter.Deserialize(sourceStream); // execute the behavior ExecuteBehavior(b); } catch (Exception ex) { OperationalTrace.WriteWarning("error reading behavior packet: " + ex.ToString()); } } else if (messageType == packet_type_comp_report) { // ignore for now, I messed this up when recording } }
private static ArbiterAdvancedRemote GetRemote() { try { return((ArbiterAdvancedRemote)CommBuilder.GetObject("ArbiterAdvancedRemote")); } catch (Exception ex) { OperationalTrace.WriteWarning("error getting arbiter remote: {0}", ex); return(null); } }
public void PushAbsolutePose(AbsolutePose pose) { lock (lockobj) { queue.Add(pose); if (!queue.VerifySort(delegate(AbsolutePose l, AbsolutePose r) { return(l.timestamp.CompareTo(r.timestamp)); })) { OperationalTrace.WriteError("absolute sort is donzoed, flushing queue"); queue.Clear(); } } }
void client_PoseRelReceived(object sender, PoseRelReceivedEventArgs e) { OperationalTrace.WriteVerbose("got relative pose for time {0}", e.PoseRelData.timestamp); CarTimestamp prevTimestamp = Services.RelativePose.CurrentTimestamp; Services.RelativePose.PushTransform(e.PoseRelData.timestamp, e.PoseRelData.transform); // get the relative transform between the previous timestamp and newest timestamp RelativeTransform transform = Services.RelativePose.GetTransform(prevTimestamp, e.PoseRelData.timestamp); lastYawRate = transform.GetRotationRate().Z; TimeoutMonitor.MarkData(OperationalDataSource.Pose); }
public void SendCompletionReport(UrbanChallenge.Behaviors.CompletionReport.CompletionReport report) { //OperationalTrace.WriteInformation("sending completion report {0}, listener {1}", report, listener == null ? "<null>" : listener.ToString()); OperationalListener l = listener; if (l != null) { try { l.OnCompletionReport(report); OperationalTrace.WriteVerbose("completion report succeeded"); } catch (Exception ex) { OperationalTrace.WriteWarning("completion report send failed: {0}", ex); } ForwardCompReport(report); } }
private void ForwardObject(object obj, bool isBehavior, MemoryStream stream) { lock (stream) { // reset the stream length stream.SetLength(0); BinaryWriter writer = new BinaryWriter(stream); // write the version writer.Write(version); // write the sequence number writer.Write(Interlocked.Increment(ref sequenceNumber)); // write the type of message writer.Write(isBehavior ? packet_type_arbiter_command : packet_type_comp_report); // write whether we're compressing or not writer.Write(packet_compressed); // create compression stream DeflateStream compressionStream = new DeflateStream(stream, CompressionMode.Compress, true); // serialize the object to the compression stream if (isBehavior) { behaviorFormatter.Serialize(compressionStream, obj); } else { compReportFormatter.Serialize(compressionStream, obj); } // close the compression stream (note: flush doesn't actually do anything, even though the documentation hints it does) compressionStream.Dispose(); // the memory stream now holds the compressed behavior // if the length is too big, send a warning and don't forward the packet if (stream.Length >= ushort.MaxValue) { OperationalTrace.WriteWarning("While forwarding object of type " + obj.GetType().Name + ", result data was too large"); } else { forwardingSocket.SendTo(stream.GetBuffer(), (int)stream.Length, SocketFlags.None, forwardingEndpoint); } } }
public static bool HandleRecoveryState() { if (Settings.TestMode) { return(false); } UpdatePosition(); int desiredResetStage = GetResetStage(); bool initialize = false; if (desiredResetStage != resetStage) { initialize = true; resetStage = desiredResetStage; OperationalTrace.WriteWarning("executing recovery stage {0}", resetStage); } bool cancelCurrentBehavior; switch (resetStage) { case 0: default: cancelCurrentBehavior = false; break; case 1: cancelCurrentBehavior = false; HandleResetStage1(initialize); break; case 2: cancelCurrentBehavior = false; HandleResetStage2(initialize); break; } return(cancelCurrentBehavior); }
private static void HandleResetStage2(bool initialize) { // if we're initializing, send the stop to both the scene estimator and local map // wait until we have confirmation that they're stopped, then restart them // wait until we get data // restart AI if (initialize) { forceTimeout = true; watchdogResetComplete = false; stage2Timer = null; watchdogResetEvent.Set(); OperationalTrace.WriteWarning("signaling restart to sensor fusion"); } else if (watchdogResetComplete) { stage2Timer = Stopwatch.StartNew(); watchdogResetComplete = false; OperationalTrace.WriteWarning("sensor fusion restart complete"); } else if (stage2Timer != null && stage2Timer.Elapsed > TimeSpan.FromSeconds(10)) { // possibly could wait until we have data to do this, but whatever OperationalTrace.WriteWarning("resetting arbiter"); // time to reset the AI stage2Timer.Reset(); try { ArbiterAdvancedRemote remote = GetRemote(); if (remote != null) { remote.Reset(); OperationalTrace.WriteWarning("arbiter reset complete"); } } catch (Exception ex) { OperationalTrace.WriteError("Error resetting AI in stage 2: {0}", ex); } } }
private static void ResetThread() { if (OperationalBuilder.BuildMode == BuildMode.Realtime) { WatchdogCommAPI.Initialize(); OperationalTrace.WriteWarning("initialized watchdog api"); while (true) { watchdogResetEvent.WaitOne(); OperationalTrace.WriteWarning("restarting local map"); while (!WatchdogCommAPI.RestartLocalMap()) { } OperationalTrace.WriteWarning("restarting scene estimator"); while (!WatchdogCommAPI.RestartSkeetEstimator()) { } watchdogResetComplete = true; forceTimeout = false; } } else { while (true) { watchdogResetEvent.WaitOne(); Thread.Sleep(100); watchdogResetComplete = true; } } }
private void ForwardedPacketReceived(IAsyncResult ar) { int bytesRead = -1; try { // end the receive operation bytesRead = forwardingSocket.EndReceive(ar); } catch (ObjectDisposedException) { // we're donzoed return; } catch (Exception) { // try again forwardingSocket.BeginReceive(listenBuffer, 0, listenBuffer.Length, SocketFlags.None, ForwardedPacketReceived, null); return; } // get the version in the first two bytes short version = BitConverter.ToInt16(listenBuffer, 0); // construct a memory stream around the buffer, starting at index 2 MemoryStream ms = new MemoryStream(listenBuffer, 2, bytesRead, false); // break on packet type if (version == 1) { ReadVersion1Packet(ms); } else { OperationalTrace.WriteWarning("Unknown forwarded packet version: " + version); } forwardingSocket.BeginReceive(listenBuffer, 0, listenBuffer.Length, SocketFlags.None, ForwardedPacketReceived, null); }
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)); } } } }
private void ObstacleThread() { for (;;) { try { SceneEstimatorUntrackedClusterCollection newUntrackedClusters = Interlocked.Exchange(ref currentUntrackedClusters, null); SceneEstimatorTrackedClusterCollection newTrackedClusters = Interlocked.Exchange(ref currentTrackedClusters, null); if (newUntrackedClusters == null && newTrackedClusters == null) { if (!Services.DebuggingService.StepMode) { newDataEvent.WaitOne(); } else { Services.DebuggingService.WaitOnSequencer(typeof(ObstaclePipeline)); } continue; } // check if we have a matching pair if (newUntrackedClusters != null) { queuedUntrackedClusters = newUntrackedClusters; } if (newTrackedClusters != null) { haveReceivedTrackedClusters = true; queuedTrackedClusters = newTrackedClusters; } if (queuedUntrackedClusters == null || (haveReceivedTrackedClusters && (queuedTrackedClusters == null || queuedTrackedClusters.timestamp != queuedUntrackedClusters.timestamp))) { continue; } Rect vehicleBox = DetermineVehicleEraseBox(); // load in the appropriate stuff to the occupancy grid useOccupancyGrid = (Services.OccupancyGrid != null && !Services.OccupancyGrid.IsDisposed); if (useOccupancyGrid) { double occup_ts = Services.OccupancyGrid.LoadNewestGrid(); if (occup_ts < 0) { useOccupancyGrid = false; } else { double delta_ts = occup_ts - queuedUntrackedClusters.timestamp; Services.Dataset.ItemAs <double>("occupancy delta ts").Add(delta_ts, queuedUntrackedClusters.timestamp); } } occupancyDeletedCount = 0; List <Obstacle> trackedObstacles; if (queuedTrackedClusters == null) { trackedObstacles = new List <Obstacle>(); } else { trackedObstacles = ProcessTrackedClusters(queuedTrackedClusters, vehicleBox); } List <Obstacle> untrackedObstacles = ProcessUntrackedClusters(queuedUntrackedClusters, trackedObstacles, vehicleBox); List <Obstacle> finalObstacles = FinalizeProcessing(trackedObstacles, untrackedObstacles, queuedUntrackedClusters.timestamp); processedObstacles = new ObstacleCollection(queuedUntrackedClusters.timestamp, finalObstacles); Services.Dataset.ItemAs <int>("occupancy deleted count").Add(occupancyDeletedCount, queuedUntrackedClusters.timestamp); queuedUntrackedClusters = null; queuedTrackedClusters = null; if (Services.DebuggingService.StepMode) { Services.DebuggingService.SetCompleted(typeof(ObstaclePipeline)); } Services.Dataset.MarkOperation("obstacle rate", LocalCarTimeProvider.LocalNow); } catch (Exception ex) { OperationalTrace.WriteError("error processing obstacles: {0}", ex); } } }
void IChannelListener.MessageArrived(string channelName, object message) { if (message is OperationalSimVehicleState) { OperationalTrace.ThreadTraceSource = TraceSource; Trace.CorrelationManager.StartLogicalOperation("simstate callback"); try { OperationalSimVehicleState state = (OperationalSimVehicleState)message; DatasetSource ds = Services.Dataset; OperationalTrace.WriteVerbose("received operational sim state, t = {0}", state.Timestamp); Services.Dataset.MarkOperation("pose rate", LocalCarTimeProvider.LocalNow); CarTimestamp now = state.Timestamp; ds.ItemAs <Coordinates>("xy").Add(state.Position, now); ds.ItemAs <double>("speed").Add(state.Speed, now); ds.ItemAs <double>("heading").Add(state.Heading, now); ds.ItemAs <double>("actual steering").Add(state.SteeringAngle, now); ds.ItemAs <TransmissionGear>("transmission gear").Add(state.TransmissionGear, now); ds.ItemAs <double>("engine torque").Add(state.EngineTorque, now); ds.ItemAs <double>("brake pressure").Add(state.BrakePressure, now); ds.ItemAs <double>("rpm").Add(state.EngineRPM, now); if (state.TransmissionGear != lastRecvTransGear) { ds.ItemAs <DateTime>("trans gear change time").Add(HighResDateTime.Now, now); lastRecvTransGear = state.TransmissionGear; } lastCarMode = state.CarMode; if (CarModeChanged != null) { CarModeChanged(this, new CarModeChangedEventArgs(lastCarMode, now)); } // build the current relative pose matrix Matrix4 relativePose = Matrix4.Translation(state.Position.X, state.Position.Y, 0) * Matrix4.YPR(state.Heading, 0, 0); relativePose = relativePose.Inverse(); // push on to the relative pose stack Services.RelativePose.PushTransform(now, relativePose); // push the current absolute pose entry AbsolutePose absPose = new AbsolutePose(state.Position, state.Heading, now); Services.AbsolutePose.PushAbsolutePose(absPose); } finally { Trace.CorrelationManager.StopLogicalOperation(); OperationalTrace.ThreadTraceSource = null; } } else if (message is SceneEstimatorUntrackedClusterCollection) { // push to obstacle pipeline Services.ObstaclePipeline.OnUntrackedClustersReceived(((SceneEstimatorUntrackedClusterCollection)message)); } else if (message is SceneEstimatorTrackedClusterCollection) { Services.ObstaclePipeline.OnTrackedClustersReceived((SceneEstimatorTrackedClusterCollection)message); } }
private List <Obstacle> FinalizeProcessing(List <Obstacle> trackedObstacles, List <Obstacle> untrackedObstacles, CarTimestamp timestamp) { List <Obstacle> finalObstacles = new List <Obstacle>(trackedObstacles.Count + untrackedObstacles.Count); finalObstacles.AddRange(trackedObstacles); finalObstacles.AddRange(untrackedObstacles); Obstacle leftObstacle = GetSideObstacle(currentLeftSideObstacles); Obstacle rightObstacle = GetSideObstacle(currentRightSideObstacles); int extraCount = 0; if (leftObstacle != null) { extraCount++; } if (rightObstacle != null) { extraCount++; } List <int> ignoredObstacles = this.lastIgnoredObstacles; OperationalObstacle[] uiObstacles = new OperationalObstacle[finalObstacles.Count + extraCount]; int i; for (i = 0; i < finalObstacles.Count; i++) { Obstacle obs = finalObstacles[i]; obs.minSpacing = min_spacing[(int)obs.obstacleClass] + ExtraSpacing; obs.desSpacing = des_spacing[(int)obs.obstacleClass] + ExtraSpacing; try { obs.cspacePolygon = Polygon.ConvexMinkowskiConvolution(conv_polygon[(int)obs.obstacleClass], obs.AvoidancePolygon); } catch (Exception) { OperationalTrace.WriteWarning("error computing minkowski convolution in finalize processing"); try { obs.cspacePolygon = obs.AvoidancePolygon.Inflate(TahoeParams.T / 2.0 + min_spacing[(int)obs.obstacleClass]); } catch (Exception) { obs.cspacePolygon = obs.AvoidancePolygon; } } OperationalObstacle uiObs = new OperationalObstacle(); uiObs.age = obs.age; uiObs.obstacleClass = obs.obstacleClass; uiObs.poly = obs.AvoidancePolygon; uiObs.headingValid = obs.absoluteHeadingValid; uiObs.heading = obs.absoluteHeading; if (ignoredObstacles != null) { uiObs.ignored = ignoredObstacles.Contains(obs.trackID); } uiObstacles[i] = uiObs; } if (leftObstacle != null) { OperationalObstacle uiObs = new OperationalObstacle(); uiObs.age = leftObstacle.age; uiObs.obstacleClass = leftObstacle.obstacleClass; uiObs.poly = leftObstacle.AvoidancePolygon; uiObstacles[i++] = uiObs; } if (rightObstacle != null) { OperationalObstacle uiObs = new OperationalObstacle(); uiObs.age = rightObstacle.age; uiObs.obstacleClass = rightObstacle.obstacleClass; uiObs.poly = rightObstacle.AvoidancePolygon; uiObstacles[i++] = uiObs; } Services.UIService.PushObstacles(uiObstacles, timestamp, "obstacles", true); return(finalObstacles); }
public SmoothingResult PlanPath(PlanningSettings settings) { SmootherOptions opts = settings.Options; // for now, just run the smoothing opts.init_heading = settings.initHeading; opts.set_init_heading = true; opts.min_init_velocity = settings.startSpeed * 0.5; opts.set_min_init_velocity = true; opts.max_init_velocity = Math.Max(settings.maxSpeed, settings.startSpeed); opts.set_max_init_velocity = true; opts.min_velocity = 0.1; opts.max_velocity = Math.Max(opts.min_velocity + 0.1, settings.maxSpeed); opts.k_max = Math.Min(TahoeParams.CalculateCurvature(-TahoeParams.SW_max, settings.startSpeed), TahoeParams.CalculateCurvature(TahoeParams.SW_max, settings.startSpeed)) * 0.97; opts.generate_details = true; // GenerateDetails; if (settings.endingHeading != null) { opts.set_final_heading = true; opts.final_heading = settings.endingHeading.Value; } else { opts.set_final_heading = false; } opts.set_final_offset = settings.endingPositionFixed; opts.final_offset_min = settings.endingPositionMin; opts.final_offset_max = settings.endingPositionMax; if (settings.maxEndingSpeed != null) { opts.set_final_velocity_max = true; opts.final_velocity_max = Math.Max(opts.min_velocity + 0.1, settings.maxEndingSpeed.Value); } else { opts.set_final_velocity_max = false; } opts.a_lat_max = 6; // create the boundary list List <UrbanChallenge.PathSmoothing.PathPoint> ret = new List <UrbanChallenge.PathSmoothing.PathPoint>(); smoother = new PathSmoother(); OperationalTrace.WriteVerbose("calling smooth path"); SmoothResult result = smoother.SmoothPath(settings.basePath, settings.targetPaths, settings.leftBounds, settings.rightBounds, opts, ret); if (result != SmoothResult.Sucess) { OperationalTrace.WriteWarning("smooth path result: {0}", result); } else { OperationalTrace.WriteVerbose("smooth path result: {0}", result); } AvoidanceDetails details = null; if (opts.generate_details) { details = new AvoidanceDetails(); details.leftBounds = settings.leftBounds; details.rightBounds = settings.rightBounds; details.smoothingDetails = smoother.GetSmoothingDetails(); LastAvoidanceDetails = details; // push out the points Coordinates[] leftPoints = new Coordinates[details.smoothingDetails.leftBounds.Length]; for (int i = 0; i < leftPoints.Length; i++) { leftPoints[i] = details.smoothingDetails.leftBounds[i].point; } Coordinates[] rightPoints = new Coordinates[details.smoothingDetails.rightBounds.Length]; for (int i = 0; i < rightPoints.Length; i++) { rightPoints[i] = details.smoothingDetails.rightBounds[i].point; } Services.UIService.PushPoints(leftPoints, settings.timestamp, "left bound points", true); Services.UIService.PushPoints(rightPoints, settings.timestamp, "right bound points", true); } //if (result == SmoothResult.Sucess) { Coordinates[] points = new Coordinates[ret.Count]; double[] speeds = new double[ret.Count]; for (int i = 0; i < ret.Count; i++) { points[i] = new Coordinates(ret[i].x, ret[i].y); speeds[i] = ret[i].v; } SmoothedPath path = new SmoothedPath(settings.timestamp, points, speeds); return(new SmoothingResult(result, path, details)); /*} * else { * SmoothedPath path = new SmoothedPath(settings.timestamp, settings.basePath, null); * * return new SmoothingResult(result, path); * }*/ }