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