public void BeginTrackingCycle(CarTimestamp timestamp) { if (path.CurrentTimestamp < timestamp) { // get the relative transform to take up to the current time path.TransformPath(Services.RelativePose.CurrentTimestamp); } }
public void Reset() { lock (lockobj) { rollingQueue.Clear(); recentTimestamp = double.NaN; } }
public PathLaneModel(CarTimestamp timestamp, LinePath path, double width) { this.timestamp = timestamp; this.path = path; this.width = width; this.laneID = "not specified"; }
private void HandleBehavior(UTurnBehavior cb) { // get the absolute transform AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(cb.TimeStamp); this.polygonTimestamp = absTransform.Timestamp; this.polygon = cb.Boundary.Transform(absTransform); this.finalLane = cb.EndingLane; this.finalSpeedCommand = cb.EndingSpeedCommand; this.stopOnLine = cb.StopOnEndingPath; this.stayOutPolygons = cb.StayOutPolygons; // run constant checking if we're supposed to stop on the final line if (stopOnLine) { checkMode = true; } else { checkMode = false; } // transform the path LinePath.PointOnPath closestPoint = cb.EndingPath.GetClosestPoint(originalPoint); // relativize the path LinePath relFinalPath = cb.EndingPath.Transform(absTransform); // get the ending orientation finalOrientation = new LineSegment(relFinalPath[closestPoint.Index], relFinalPath[closestPoint.Index + 1]); Services.UIService.PushLineList(cb.EndingPath, cb.TimeStamp, "original path1", false); }
public BadPointOnPath(Coordinates xy, IRelativePath path, CarTimestamp timestamp, object data) { this.xy = xy; this.path = path; this.data = data; this.timestamp = timestamp; }
public void BeginTrackingCycle(CarTimestamp timestamp) { if (SpeedController.config != SpeedControllerConfig.Normal) { SpeedController.config = SpeedControllerConfig.Normal; } }
public void HandleMessage(byte[] message, object target, DatasetSource ds) { BigEndianBinaryReader reader = new BigEndianBinaryReader(message); // read in the current time CarTimestamp time = new CarTimestamp(reader.ReadUInt16(), reader.ReadInt32()); // read the feedback type byte feedbackType = reader.ReadByte(); // read the length ushort len = reader.ReadUInt16(); // dispatch if we can FeedbackType type; if (types.TryGetValue((int)feedbackType, out type)) { object[] vals = type.MapMessage(target, ds, time, reader); if (logWriter != null) { logWriter.Write(time); logWriter.Write(","); logWriter.Write(feedbackType); logWriter.Write(","); for (int i = 0; i < vals.Length - 1; i++) { logWriter.Write(vals[i]); logWriter.Write(","); } logWriter.WriteLine(vals[vals.Length - 1]); } } }
private List <Polygon> GetObstacles(CarTimestamp curTimestamp) { int total = 0; ObstacleCollection obstacles = Services.ObstaclePipeline.GetProcessedObstacles(curTimestamp, Services.BehaviorManager.SAUDILevel); total += obstacles.obstacles.Count; if (stayOutPolygons != null) { total += stayOutPolygons.Count; } List <Polygon> polys = new List <Polygon>(total); foreach (Obstacle obs in obstacles.obstacles) { polys.Add(obs.AvoidancePolygon); } // transform the stay-out polygons if (stayOutPolygons != null) { AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp); foreach (Polygon poly in stayOutPolygons) { polys.Add(poly.Transform(absTransform)); } } return(polys); }
protected List <Obstacle> GetObstacles(CarTimestamp curTimestamp) { ObstacleCollection col = Services.ObstaclePipeline.GetProcessedObstacles(curTimestamp, SAUDILevel.None); AbsoluteTransformer transform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp).Invert(); List <Obstacle> ret = new List <Obstacle>(col.obstacles.Count); foreach (Obstacle obs in col.obstacles) { Obstacle newObs = obs.ShallowClone(); if (newObs.cspacePolygon != null) { newObs.cspacePolygon = newObs.cspacePolygon.Transform(transform); } if (newObs.extrudedPolygon != null) { newObs.extrudedPolygon = newObs.extrudedPolygon.Transform(transform); } if (newObs.obstaclePolygon != null) { newObs.obstaclePolygon = newObs.obstaclePolygon.Transform(transform); } if (newObs.predictedPolygon != null) { newObs.predictedPolygon = newObs.predictedPolygon.Transform(transform); } ret.Add(newObs); } return(ret); }
public static void OnRoadBearing(CarTimestamp timestamp, double angle, double confidence) { if (currentConfidence == 0) { currentAngle = angle; currentConfidence = confidence; } else if (confidence == 0) { currentConfidence *= 0.5; } else { currentAngle = alpha * angle + (1 - alpha) * currentAngle; currentConfidence = confidence; } LineList roadBearing = new LineList(2); roadBearing.Add(new Coordinates(2, 0).Rotate(currentAngle)); roadBearing.Add(new Coordinates(10, 0).Rotate(currentAngle)); Services.UIService.PushLineList(roadBearing, timestamp, "road bearing", true); Services.Dataset.ItemAs <double>("road bearing confidence").Add(confidence, timestamp); }
public LinearizationOptions(double startDist, double endDist, CarTimestamp timestamp) { this.StartDistance = startDist; this.EndDistance = endDist; this.Timestamp = timestamp; this.LaneShiftDistance = 0; }
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]); } }
public void OnAllFast( CarTimestamp ct, double wheelFL, double wheelFR, double wheelRR, double wheelRL, double rpm, double steerAngle, TransmissionGear transGear, int transDir, double actTorque, double reqTorque, int cylinderDeactivation, int pedalPos, double brakePressure) { OnWheelspeed(wheelFL, wheelFR, wheelRR, wheelRL, ct); OnBrake(brakePressure, ct); if (transGear != lastRecvTransmissionGear) { Services.Dataset.ItemAs <DateTime>("trans gear change time").Add(HighResDateTime.Now, ct); lastRecvTransmissionGear = transGear; } }
void TrackedDistance_DataValueAdded(object sender, SourceDataValueAddedEventArgs<double> e) { // check if we received something before if (double.IsNaN(lastTime.ts)) { // we haven't received anything, initialize starting time lastTime = e.Time; } else if (e.Time < lastTime) { //OperationalTrace.WriteWarning("resetting tracked distanace: event time {0}, last time {1}", e.Time, lastTime); // timestamp rollover/reset // clear everything out lock (lockobj) { queue.Clear(); lastTime = e.Time; lastDist = 0; } } else { // calculate dt double dt = e.Time.ts - lastTime.ts; // calculate delta distance double dx = Math.Abs(e.Value)*dt; // get the lock lock (lockobj) { lastDist += dx; lastTime = e.Time; //OperationalTrace.WriteVerbose("adding dist {0}, time {1}", lastDist, lastTime); queue.Add(new TrackedDistanceItem(lastTime, lastDist)); } } }
public void Initialize(Behavior b) { SimpleStayInLaneBehavior sb = (SimpleStayInLaneBehavior)b; // store the base path basePath = ConvertPath(sb.BasePath); // get the lower, upper bound leftBound = FindBoundary(sb.LaneWidth / 2); rightBound = FindBoundary(-sb.LaneWidth / 2); // convert everything to be vehicle relative AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(); pathTime = absTransform.Timestamp; basePath = basePath.Transform(absTransform); leftBound = leftBound.Transform(absTransform); rightBound = rightBound.Transform(absTransform); // send the left and right bounds Services.UIService.PushRelativePath(leftBound, pathTime, "left bound"); Services.UIService.PushRelativePath(rightBound, pathTime, "right bound"); maxSpeed = sb.MaxSpeed; obstacleManager = new ObstacleManager(); // hik }
public void PushPolygons(Polygon[] polygons, CarTimestamp timestamp, string name, bool relative) { try { if (polygons == null) { return; } if (relative) { Polygon[] transPolygons = new Polygon[polygons.Length]; AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(timestamp).Invert(); timestamp = absTransform.Timestamp; for (int i = 0; i < polygons.Length; i++) { transPolygons[i] = polygons[i].Transform(absTransform); } polygons = transPolygons; } Services.Dataset.ItemAs <Polygon[]>(name).Add(polygons, timestamp); } catch (Exception ex) { OperationalLayer.Tracing.OperationalTrace.WriteWarning("could not send polygon data to ui: {0}", ex.Message); } }
public void PushObstacles(OperationalObstacle[] obstacles, CarTimestamp timestamp, string name, bool relative) { try { if (obstacles == null || obstacles.Length == 0) { return; } if (relative) { OperationalObstacle[] transformObstacles = new OperationalObstacle[obstacles.Length]; AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(timestamp).Invert(); timestamp = absTransform.Timestamp; for (int i = 0; i < obstacles.Length; i++) { transformObstacles[i] = obstacles[i].ShallowClone(); transformObstacles[i].poly = obstacles[i].poly.Transform(absTransform); } obstacles = transformObstacles; } Services.Dataset.ItemAs <OperationalObstacle[]>(name).Add(obstacles, timestamp); } catch (Exception) { } }
public void PushTransform(CarTimestamp timestamp, Matrix4 transform) { lock (lockobj) { TransformEntry te = new TransformEntry(timestamp, transform); rollingQueue.Add(te); // check if this is the most recent timestamp if (!double.IsNaN(recentTimestamp.ts) && timestamp < recentTimestamp) { // do a reverse bubble-sort int i = rollingQueue.Count - 2; // find the insertion point while (i >= 0 && rollingQueue[i].timestamp > timestamp) { // shift the entry up rollingQueue[i + 1] = rollingQueue[i]; i--; } // i+1 contains the empty slot to insert at rollingQueue[i + 1] = te; } else { // update the recent timestamp recentTimestamp = timestamp; } if (!rollingQueue.VerifySort(delegate(TransformEntry l, TransformEntry r) { return(l.timestamp.CompareTo(r.timestamp)); })) { Trace.TraceError("relative transform sort is donzoed, flushing queue"); Reset(); } } }
public SmoothingResult PlanPath(LineList basePath, LineList targetPath, IList<LineList> leftBounds, IList<LineList> rightBounds, double initHeading, double maxSpeed, double startSpeed, double? endingHeading, CarTimestamp time, bool endingOffsetFixed) { // create the boundary list List<Boundary> upperBound = new List<Boundary>(); foreach (LineList leftBound in leftBounds) { Boundary ub0 = new Boundary(); ub0.Coords = leftBound; ub0.DesiredSpacing = 0.5; ub0.MinSpacing = 0.1; ub0.Polygon = false; upperBound.Add(ub0); } List<Boundary> lowerBound = new List<Boundary>(); foreach (LineList rightBound in rightBounds) { Boundary lb0 = new Boundary(); lb0.Coords = rightBound; lb0.DesiredSpacing = 0.5; lb0.MinSpacing = 0.1; lb0.Polygon = false; lowerBound.Add(lb0); } return PlanPath(basePath, targetPath, upperBound, lowerBound, initHeading, maxSpeed, startSpeed, endingHeading, time, endingOffsetFixed); }
public void Initialize(Behavior b) { SimpleStayInLaneBehavior sb = (SimpleStayInLaneBehavior)b; // store the base path basePath = ConvertPath(sb.BasePath); // get the lower, upper bound leftBound = FindBoundary(sb.LaneWidth/2); rightBound = FindBoundary(-sb.LaneWidth/2); // convert everything to be vehicle relative AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(); pathTime = absTransform.Timestamp; basePath = basePath.Transform(absTransform); leftBound = leftBound.Transform(absTransform); rightBound = rightBound.Transform(absTransform); // send the left and right bounds Services.UIService.PushRelativePath(leftBound, pathTime, "left bound"); Services.UIService.PushRelativePath(rightBound, pathTime, "right bound"); maxSpeed = sb.MaxSpeed; obstacleManager = new ObstacleManager(); // hik }
public double GetDistanceTravelled(CarTimestamp t0, CarTimestamp t1) { TrackedDistanceItem item0 = null; TrackedDistanceItem item1 = null; lock (lockobj) { // find the location of the starting and stopping time int i0 = queue.Count-1; while (i0 >= 0 && queue[i0].timestamp > t0) { i0--; } if (i0 >= 0) { item0 = queue[i0]; } int i1 = queue.Count-1; while (i1 >= 0 && queue[i1].timestamp > t1) { i1--; } if (i1 >= 0) { item1 = queue[i1]; } } if (item0 == null || item1 == null) throw new InvalidOperationException(string.Format("Requested timestamps {0:F4}->{1:F4} do not exist in queue", t0, t1)); //OperationalTrace.WriteVerbose("looking for {0}->{1}, got {2}->{3}, distance {4}->{5}, travelled {6}", t0, t1, item0.timestamp, item1.timestamp, item0.distance, item1.distance, item1.distance - item0.distance); return item1.distance - item0.distance; }
public CombinedLaneModel(LinePath centerlinePath, LinePath leftBound, LinePath rightBound, double nominalWidth, CarTimestamp timestamp) { this.centerlinePath = centerlinePath; this.leftBound = leftBound; this.rightBound = rightBound; this.timestamp = timestamp; this.nominalWidth = nominalWidth; }
public LocalRoadModel(CarTimestamp timestamp, double modelProbability, LocalLaneModel centerLane, LocalLaneModel leftLane, LocalLaneModel rightLane) { this.timestamp = timestamp; this.modelProbability = modelProbability; this.centerLane = centerLane; this.leftLane = leftLane; this.rightLane = rightLane; }
public void OnWheelspeed(double fl, double fr, double rr, double rl, CarTimestamp ct) { ws_count++; if (useWheelspeed) { Services.Dataset.ItemAs <double>("speed").Add((rr + rl) / 2.0 * (10.0 / 36.0), ct); } }
public TrackedDistance(int capacity) { lastDist = 0; lastTime = new CarTimestamp(double.NaN); lockobj = new object(); queue = new RollingQueue <TrackedDistanceItem>(capacity); Services.Dataset.ItemAs <double>("speed").DataValueAdded += TrackedDistance_DataValueAdded; }
public TrackedDistance(int capacity) { lastDist = 0; lastTime = new CarTimestamp(double.NaN); lockobj = new object(); queue = new RollingQueue<TrackedDistanceItem>(capacity); Services.Dataset.ItemAs<double>("speed").DataValueAdded += TrackedDistance_DataValueAdded; }
public void OnBrake(double value, CarTimestamp ct) { // filter out bad values (70 chosen as a cutoff for now) if (value < 70) { // add the item to the dataset Services.Dataset.ItemAs <double>("brake pressure").Add(value, ct); } }
public AbsolutePose GetAbsolutePose(CarTimestamp timestamp) { if (Settings.UsePosteriorPose) { return Services.AbsolutePosteriorPose.GetAbsolutePose(timestamp); } else { return Services.AbsolutePose.GetAbsolutePose(timestamp); } }
private bool CheckGeneralShape(LinePath rndfPath, CarTimestamp rndfPathTimestamp, LocalLaneModel centerLaneModel) { // bad newz bears if (centerLaneModel.LanePath == null || centerLaneModel.LanePath.Count < 2) { return(false); } // project the lane model's path into the rndf path's timestamp RelativeTransform relTransform = Services.RelativePose.GetTransform(localRoadModel.Timestamp, rndfPathTimestamp); LinePath laneModelPath = centerLaneModel.LanePath.Transform(relTransform); // get the zero point of the lane model path LinePath.PointOnPath laneZeroPoint = laneModelPath.ZeroPoint; // get the first waypoint on the RNDF path LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint; Coordinates firstWaypoint = rndfPath[rndfZeroPoint.Index + 1]; // get the projection of the first waypoint onto the lane path LinePath.PointOnPath laneFirstWaypoint = laneModelPath.GetClosestPoint(firstWaypoint); // get the offset vector Coordinates offsetVector = laneFirstWaypoint.Location - firstWaypoint; // start iterating through the waypoints forward and project them onto the rndf path for (int i = rndfZeroPoint.Index + 1; i < rndfPath.Count; i++) { // get the waypoint Coordinates waypoint = rndfPath[i]; // adjust by the first waypoint offset waypoint += offsetVector; // project onto the lane model LinePath.PointOnPath laneWaypoint = laneModelPath.GetClosestPoint(waypoint); // check the distance from the zero point on the lane model if (laneModelPath.DistanceBetween(laneZeroPoint, laneWaypoint) > road_model_max_dist) { break; } // check the devation from the rndf double deviation = waypoint.DistanceTo(laneWaypoint.Location); // if the deviation is over some threshold, then we reject the model if (deviation > road_deviation_reject_threshold) { return(false); } } // we got this far, so this stuff is OK return(true); }
public PoseListener() { client = new PoseClient(IPAddress.Parse("239.132.1.33"), 4839); client.PoseAbsReceived += client_PoseAbsReceived; client.PoseRelReceived += client_PoseRelReceived; lastPacketTime = new CarTimestamp(-10000); BiasEstimator = new HeadingBiasEstimator(); }
public static void SmoothAndTrack(LinePath basePath, bool useTargetPath, LinePath leftBound, LinePath rightBound, double maxSpeed, double?endingHeading, bool endingOffsetBound, CarTimestamp curTimestamp, bool doAvoidance, SpeedCommand speedCommand, CarTimestamp behaviorTimestamp, string commandLabel, ref bool cancelled, ref LinePath previousSmoothedPath, ref CarTimestamp previousSmoothedPathTimestamp, ref double?approachSpeed) { SmoothAndTrack(basePath, useTargetPath, new LinePath[] { leftBound }, new LinePath[] { rightBound }, maxSpeed, endingHeading, endingOffsetBound, curTimestamp, doAvoidance, speedCommand, behaviorTimestamp, commandLabel, ref cancelled, ref previousSmoothedPath, ref previousSmoothedPathTimestamp, ref approachSpeed); }
public void MarkOperation(string key, CarTimestamp ts) { RunRateDataItemSource item = items[key] as RunRateDataItemSource; if (item == null) { throw new KeyNotFoundException(); } item.Mark(ts); }
public AbsoluteTransformer GetAbsoluteTransformer(CarTimestamp timestamp) { AbsolutePose pose; if (Settings.UsePosteriorPose) { pose = Services.AbsolutePosteriorPose.GetAbsolutePose(timestamp); } else { pose = Services.AbsolutePose.GetAbsolutePose(timestamp); } return new AbsoluteTransformer(pose.xy, pose.heading, pose.timestamp); }
public AbsolutePose GetAbsolutePose(CarTimestamp timestamp) { if (Settings.UsePosteriorPose) { return(Services.AbsolutePosteriorPose.GetAbsolutePose(timestamp)); } else { return(Services.AbsolutePose.GetAbsolutePose(timestamp)); } }
public void BeginTrackingCycle(CarTimestamp timestamp) { // transform the path TransformPath(timestamp); // reset to normal speed controller config if (SpeedController.config != SpeedControllerConfig.Normal) { SpeedController.config = SpeedControllerConfig.Normal; } }
public void BeginTrackingCycle(CarTimestamp timestamp) { if (SpeedController.config != SpeedControllerConfig.Stopping) { SpeedController.Reset(); SpeedController.config = SpeedControllerConfig.Stopping; } distProvider.Transform(timestamp); curTimestamp = timestamp; }
public OperationalVehicleState(double speed, double steeringAngle, TransmissionGear transGear, double pitch, double brakePressure, double engineTorque, double engineRPM, double pedalPosition, CarTimestamp timestamp) { this.speed = speed; this.steeringAngle = steeringAngle; this.transGear = transGear; this.pitch = pitch; this.engineTorque = engineTorque; this.engineRPM = engineRPM; this.brakePressure = brakePressure; this.pedalPosition = pedalPosition; this.timestamp = timestamp; }
private void SendLaneModelToUI(ILaneModel laneModel, CarTimestamp ts) { // get the center, left and right bounds //LinearizationOptions opts = new LinearizationOptions(0, 50, ts); //LinePath finalCenter = laneModel.LinearizeCenterLine(opts); //LinePath finalLeft = laneModel.LinearizeLeftBound(opts); //LinePath finalRight = laneModel.LinearizeRightBound(opts); //Services.UIService.PushLineList(finalCenter, ts, "lane center line", true); //Services.UIService.PushLineList(finalLeft, ts, "lane left bound", true); //Services.UIService.PushLineList(finalRight, ts, "lane right bound", true); }
public void SetLaneModel(IEnumerable<ILaneModel> models) { Dictionary<string, ILaneModel> newLanes = new Dictionary<string, ILaneModel>(); foreach (ILaneModel l in models) { lastTime = l.Timestamp; newLanes.Add(l.LaneID, l); } lock (lockobj) { laneModels = newLanes; } }
public void GetLaneChangeModels(LinePath startingRndfPath, double startingRndfPathWidth, int startingNumLanesLeft, int startingNumLanesRight, LinePath endingRndfPath, double endingRndfPathWidth, bool changeLeft, CarTimestamp rndfPathTimestamp, out ILaneModel startingLaneModel, out ILaneModel endingLaneModel) { LocalRoadModel localRoadModel = this.localRoadModel; // get the selected lane for the starting path int startingSelectedLane = SelectLane(localRoadModel, startingRndfPath, startingRndfPathWidth, rndfPathTimestamp, startingNumLanesLeft, startingNumLanesRight); // calculate the number of lanes left and right for the ending lane int endingNumLanesLeft = startingNumLanesLeft; int endingNumLanesRight = startingNumLanesRight; if (changeLeft) { endingNumLanesLeft--; endingNumLanesRight++; } else { endingNumLanesLeft++; endingNumLanesRight--; } // get the selected lane for the ending path int endingSelectedLane = SelectLane(localRoadModel, endingRndfPath, endingRndfPathWidth, rndfPathTimestamp, endingNumLanesLeft, endingNumLanesRight); // check if either is invalid or the difference does not line up with the change direction int deltaExpected = changeLeft ? 1 : -1; // starting - ending if (startingSelectedLane < 0 || endingSelectedLane < 0 || (startingSelectedLane - endingSelectedLane) != deltaExpected) { // this is some invalid stuff // we won't use either of them since we're not sure which one is truly valid startingLaneModel = GetLaneModel(localRoadModel, -1, startingRndfPath, startingRndfPathWidth, rndfPathTimestamp); endingLaneModel = GetLaneModel(localRoadModel, -1, endingRndfPath, endingRndfPathWidth, rndfPathTimestamp); // mark that we did reject didRejectLast = true; } else { // looks like the lane model selection was valid startingLaneModel = GetLaneModel(localRoadModel, startingSelectedLane, startingRndfPath, startingRndfPathWidth, rndfPathTimestamp); endingLaneModel = GetLaneModel(localRoadModel, endingSelectedLane, endingRndfPath, endingRndfPathWidth, rndfPathTimestamp); // mark that we did not reject didRejectLast = false; } // figure out what we want to send to the ui // for now, just send starting lane model SendLaneModelToUI(startingLaneModel, rndfPathTimestamp); }
public OperationalSimVehicleState(Coordinates position, double speed, double heading, double steeringAngle, TransmissionGear transGear, double engineTorque, double brakePressure, double engineRPM, CarMode carMode, double stopLineDistance, bool stoplineFound, CarTimestamp timestamp) { this.position = position; this.speed = speed; this.heading = heading; this.steeringAngle = steeringAngle; this.transGear = transGear; this.engineTorque = engineTorque; this.brakePressure = brakePressure; this.engineRPM = engineRPM; this.carMode = carMode; this.stopLineDistance = stopLineDistance; this.stoplineFound = stoplineFound; this.timestamp = timestamp; }
public object[] MapMessage(object target, DatasetSource ds, CarTimestamp ts, BigEndianBinaryReader reader) { // reader will be positioned at start of payload int nextra = 0; if (methodHasTimestamp) nextra++; if (methodHasMsgType) nextra++; object[] vals = new object[fields.Count]; object[] param = null; if (method != null) { if (methodIsObjectArray) { param = new object[1+nextra]; } else { param = new object[fields.Count + nextra]; } if (methodHasTimestamp) { param[0] = ts; if (methodHasMsgType) param[1] = id; } else if (methodHasMsgType) { param[0] = id; } } for (int i = 0; i < fields.Count; i++) { object val = fields[i].MapField(reader, ds, ts); if (param != null && !methodIsObjectArray) param[i + nextra] = val; vals[i] = val; } if (methodIsObjectArray && param != null) { param[nextra] = vals; } if (method != null) { method.Invoke(target, param); } return vals; }
public RelativeTransform GetTransform(CarTimestamp timestampOrigin, CarTimestamp timestampEnd) { lock (lockobj) { // find the best origin timestamp // this will be the entry just less than timestampOrigin int i_origin = rollingQueue.Count-1; while (i_origin >= 0 && rollingQueue[i_origin].timestamp > timestampOrigin) { i_origin--; } // fidn the best ending timestamp int i_end = rollingQueue.Count-1; while (i_end >= 0 && rollingQueue[i_end].timestamp > timestampEnd) { i_end--; } // check if either transform isn't found if (i_origin == -1 || i_end == -1) { throw new TransformationNotFoundException("Requested transformation does not exist in the queued data: " + timestampOrigin.ts.ToString("F5") + "->" + timestampEnd.ts.ToString("F5")); } // check that the timestamps we chose make sense // it should be the case that the transform chosen at or before the timestamp and the next // transform is after the timestamp if (rollingQueue[i_origin].timestamp > timestampOrigin) { // this is some stuff Console.Write("origin index bad: " + i_origin + ", entry timestamp: " + rollingQueue[i_origin].timestamp + ", requested timestamp: " + timestampOrigin); Console.WriteLine(); } if (i_origin < rollingQueue.Count-1) { Debug.Assert(rollingQueue[i_origin+1].timestamp > timestampOrigin); } Debug.Assert(rollingQueue[i_end].timestamp <= timestampEnd); if (i_end < rollingQueue.Count-1) { Debug.Assert(rollingQueue[i_end+1].timestamp > timestampEnd); } // build the relative transform Matrix4 transform = rollingQueue[i_end].transform*rollingQueue[i_origin].transform.Inverse(); // return the stuff return new RelativeTransform(timestampOrigin, timestampEnd, transform); } }
public PathLaneModel(CarTimestamp timestamp, PathRoadModel.LaneEstimate lane) { this.path = lane.path; int dotIndex = lane.paritionID.LastIndexOf('.'); if (dotIndex == -1) { this.laneID = lane.paritionID + ".not a lane"; } else { this.laneID = lane.paritionID.Substring(0, dotIndex); } this.width = lane.width; if (this.width < TahoeParams.T + 2) { this.width = TahoeParams.T + 2; } this.timestamp = timestamp; }
public override void Initialize(Behavior b) { base.Initialize(b); Services.ObstaclePipeline.ExtraSpacing = 0; Services.ObstaclePipeline.UseOccupancyGrid = true; ChangeLaneBehavior cb = (ChangeLaneBehavior)b; Services.UIService.PushAbsolutePath(cb.BackupStartLanePath, cb.TimeStamp, "original path1"); Services.UIService.PushAbsolutePath(cb.BackupTargetLanePath, cb.TimeStamp, "original path2"); startTimestamp = b.TimeStamp; HandleBehavior(cb); BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "in change lanes -- initializing, start timestamp {0}", b.TimeStamp); }
public void Update(double yawRate, double heading, Vector3 velENU, double dt, CarTimestamp ts) { bool inReverse = Services.StateProvider.GetVehicleState().transGear == UrbanChallenge.Common.Vehicle.TransmissionGear.Reverse; // predict forward the state double lamda = Math.Exp(-dt/auto_corr_time); mean *= lamda; var = lamda*lamda*var + (1 - lamda*lamda)*nom_heading_sigma*nom_heading_sigma; // calculate the measurement double ve = velENU.X; double vn = velENU.Y; double v = Math.Sqrt(ve*ve + vn*vn); // ignore if we're going less than 2.5 m/s (approx 5 mph) if (v < 2.5) return; // calculate measurement double vel_heading = Math.Atan2(vn, ve); if (inReverse) { vel_heading += Math.PI; } vel_heading = MathUtil.WrapAngle(vel_heading, heading); Services.Dataset.ItemAs<double>("vel heading").Add(vel_heading, ts); double heading_bias = vel_heading - heading; // calculate measurement variance double measurement_var = measurement_sigma_0*measurement_sigma_0; if (v < 7.5) { // add in 3 deg noise at 2.5, 0 at 7.5 double sigma_v_fac = Math.Pow(5*Math.PI/180.0,2)/5; measurement_var += sigma_v_fac*(7.5-v); } // add in noise for yaw rate measurement_var += Math.Abs(yawRate)*measurement_sigma_wz; double innov = heading_bias - mean; double innov_var = var + measurement_var; double gain = var/innov_var; mean += gain*innov; var *= (1-gain); }
public static void OnRoadBearing(CarTimestamp timestamp, double angle, double confidence) { if (currentConfidence == 0) { currentAngle = angle; currentConfidence = confidence; } else if (confidence == 0) { currentConfidence *= 0.5; } else { currentAngle = alpha*angle + (1-alpha)*currentAngle; currentConfidence = confidence; } LineList roadBearing = new LineList(2); roadBearing.Add(new Coordinates(2, 0).Rotate(currentAngle)); roadBearing.Add(new Coordinates(10, 0).Rotate(currentAngle)); Services.UIService.PushLineList(roadBearing, timestamp, "road bearing", true); Services.Dataset.ItemAs<double>("road bearing confidence").Add(confidence, timestamp); }
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]; } }
public static double GetPlanningDistance(CarTimestamp curTimestamp, SpeedCommand speedCommand, CarTimestamp speedCommandTimestamp) { if (speedCommand is ScalarSpeedCommand) { // figure out the commanded speed and plan for the stopping distance double commandedSpeed = ((ScalarSpeedCommand)speedCommand).Speed; // calculate expected stopping distance (actual deceleration at 2 m/s^2 + system latency + tahoe rear-axle to front-bumper length) double planningDist = commandedSpeed*commandedSpeed/(2*planning_dist_decel) + commandedSpeed*planning_dist_sys_latency + TahoeParams.FL; //if (planningDist < TahoeParams.FL + TahoeParams.VL) { // planningDist = TahoeParams.FL + TahoeParams.VL; //} if (planningDist < planning_dist_min) { planningDist = planning_dist_min; } else if (planningDist > planning_dist_max) { planningDist = planning_dist_max; } return planningDist; } else if (speedCommand is StopAtDistSpeedCommand) { // transform the distance to the current timestamp double origDist = ((StopAtDistSpeedCommand)speedCommand).Distance; double remainingDist = origDist - Services.TrackedDistance.GetDistanceTravelled(speedCommandTimestamp, curTimestamp); if (remainingDist < 0) { remainingDist = 0; } return remainingDist + TahoeParams.FL; } else if (speedCommand is StopAtLineSpeedCommand) { double remainingDist = Services.Stopline.DistanceToStopline(); return remainingDist; } else { throw new InvalidOperationException(); } }
public void PushCircle(Circle circle, CarTimestamp timestamp, string name, bool relative) { try { if (relative) { AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(timestamp).Invert(); timestamp = absTransform.Timestamp; circle = circle.Transform(absTransform); } Services.Dataset.ItemAs<Circle>(name).Add(circle, timestamp); } catch (Exception ex) { OperationalLayer.Tracing.OperationalTrace.WriteWarning("could not send circle data to ui: {0}", ex.Message); } }
public void PushAbsolutePath(LineList list, CarTimestamp timestamp, string name) { Services.Dataset.ItemAs<LineList>(name).Add(list, timestamp); }