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 BadPointOnPath(Coordinates xy, IRelativePath path, CarTimestamp timestamp, object data)
{
this.xy = xy;
this.path = path;
this.data = data;
this.timestamp = 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;
}
}
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]);
}
}
}
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 LinearizationOptions(double startDist, double endDist, CarTimestamp timestamp)
{
this.StartDistance = startDist;
this.EndDistance = endDist;
this.Timestamp = timestamp;
this.LaneShiftDistance = 0;
}
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 PathLaneModel(CarTimestamp timestamp, LinePath path, double width)
{
this.timestamp = timestamp;
this.path = path;
this.width = width;
this.laneID = "not specified";
}
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 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 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 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 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;
}
public void BeginTrackingCycle(CarTimestamp timestamp)
{
if (SpeedController.config != SpeedControllerConfig.Stopping)
{
SpeedController.Reset();
SpeedController.config = SpeedControllerConfig.Stopping;
}
distProvider.Transform(timestamp);
curTimestamp = 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);
}
