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