protected override bool IsOffRoad()
{
// update the rndf path
RelativeTransform relTransfrom = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
LinePath curRndfPath = rndfPath.Transform(relTransfrom);
LinePath.PointOnPath closestPoint = curRndfPath.ZeroPoint;
// determine if our heading is off-set from the lane greatly
LineSegment curSegment = curRndfPath.GetSegment(closestPoint.Index);
double relativeAngle = Math.Abs(curSegment.UnitVector.ArcTan);
if (relativeAngle > 30 * Math.PI / 180.0)
{
return(true);
}
// check the offtrack distance
if (Math.Abs(closestPoint.OfftrackDistance(Coordinates.Zero)) / (rndfPathWidth / 2.0) > 1)
{
return(true);
}
return(false);
}
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);
}
protected override Paint GetPaintInner(Windows.Foundation.Rect destinationRect)
{
var paint = new Paint();
// Android LinearGradient requires two ore more stop points.
if (GradientStops.Count >= 2)
{
var colors = GradientStops.Select(s => ((Android.Graphics.Color)s.Color).ToArgb()).ToArray();
var locations = GradientStops.Select(s => (float)s.Offset).ToArray();
var width = destinationRect.Width;
var height = destinationRect.Height;
var transform = RelativeTransform?.ToNative(size: new Windows.Foundation.Size(width, height), isBrush: true);
//Matrix .MapPoints takes an array of floats
var pts = new[] { StartPoint, EndPoint }
.Select(p => new float[] { (float)(p.X * width), (float)(p.Y * height) })
.SelectMany(p => p)
.ToArray();
transform?.MapPoints(pts);
var shader = new LinearGradient(pts[0], pts[1], pts[2], pts[3], colors, locations, Shader.TileMode.Mirror);
paint.SetShader(shader);
}
return(paint);
}
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);
}
protected override bool BeginRenderOpacityBrushOverride(Texture tex, RenderContext renderContext)
{
if (_gradientBrushTexture == null || _refresh)
{
_gradientBrushTexture = BrushCache.Instance.GetGradientBrush(GradientStops);
if (_gradientBrushTexture == null)
{
return(false);
}
}
Matrix finalTransform = renderContext.Transform.Clone();
if (_refresh)
{
_refresh = false;
_effect = ContentManager.Instance.GetEffect(EFFECT_LINEAROPACITYGRADIENT);
g_startpoint = new float[] { StartPoint.X, StartPoint.Y };
g_endpoint = new float[] { EndPoint.X, EndPoint.Y };
if (MappingMode == BrushMappingMode.Absolute)
{
g_startpoint[0] /= _vertsBounds.Width;
g_startpoint[1] /= _vertsBounds.Height;
g_endpoint[0] /= _vertsBounds.Width;
g_endpoint[1] /= _vertsBounds.Height;
}
g_framesize = new float[] { _vertsBounds.Width, _vertsBounds.Height };
if (RelativeTransform != null)
{
Matrix m = RelativeTransform.GetTransform();
m.Transform(ref g_startpoint[0], ref g_startpoint[1]);
m.Transform(ref g_endpoint[0], ref g_endpoint[1]);
}
}
SurfaceDescription desc = tex.GetLevelDescription(0);
float[] g_LowerVertsBounds = new float[] { _vertsBounds.Left / desc.Width, _vertsBounds.Top / desc.Height };
float[] g_UpperVertsBounds = new float[] { _vertsBounds.Right / desc.Width, _vertsBounds.Bottom / desc.Height };
_effect.Parameters[PARAM_TRANSFORM] = GetCachedFinalBrushTransform();
_effect.Parameters[PARAM_OPACITY] = (float)(Opacity * renderContext.Opacity);
_effect.Parameters[PARAM_STARTPOINT] = g_startpoint;
_effect.Parameters[PARAM_ENDPOINT] = g_endpoint;
_effect.Parameters[PARAM_FRAMESIZE] = g_framesize;
_effect.Parameters[PARAM_ALPHATEX] = _gradientBrushTexture.Texture;
_effect.Parameters[PARAM_UPPERVERTSBOUNDS] = g_UpperVertsBounds;
_effect.Parameters[PARAM_LOWERVERTSBOUNDS] = g_LowerVertsBounds;
GraphicsDevice.Device.SetSamplerState(0, SamplerState.AddressU, SpreadAddressMode);
_effect.StartRender(tex, finalTransform);
return(true);
}
private void ProcessReverse()
{
double planningDistance = GetPlanningDistance();
// update the rndf path
RelativeTransform relTransfrom = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
LinePath curRndfPath = rndfPath.Transform(relTransfrom);
Console.WriteLine("cur rndf path count: " + curRndfPath.Count + ", " + curRndfPath.PathLength);
Console.WriteLine("cur rndf path zero point valid: " + curRndfPath.ZeroPoint.Valid + ", loc: " + curRndfPath.ZeroPoint.Location + ", index: " + curRndfPath.ZeroPoint.Index);
Console.WriteLine("planning dist: " + planningDistance + ", stop dist: " + GetSpeedCommandStopDistance());
// get the path in reverse
double dist = -(planningDistance + TahoeParams.RL + 2);
LinePath targetPath = curRndfPath.SubPath(curRndfPath.ZeroPoint, ref dist);
if (dist < 0)
{
targetPath.Add(curRndfPath[0] - curRndfPath.GetSegment(0).Vector.Normalize(-dist));
}
Console.WriteLine("target path count: " + targetPath.Count + ", " + targetPath.PathLength);
Console.WriteLine("target path zero point valud: " + targetPath.ZeroPoint.Valid);
// generate a box by shifting the path
LinePath leftBound = targetPath.ShiftLateral(rndfPathWidth / 2.0);
LinePath rightBound = targetPath.ShiftLateral(-rndfPathWidth / 2.0);
double leftStartDist, rightStartDist;
GetLaneBoundStartDists(targetPath, rndfPathWidth, out leftStartDist, out rightStartDist);
leftBound = leftBound.RemoveBefore(leftBound.AdvancePoint(leftBound.StartPoint, leftStartDist));
rightBound = rightBound.RemoveBefore(rightBound.AdvancePoint(rightBound.StartPoint, rightStartDist));
AddTargetPath(targetPath, 0.0025);
AddLeftBound(leftBound, false);
AddRightBound(rightBound, false);
avoidanceBasePath = targetPath;
double targetDist = Math.Max(targetPath.PathLength - (TahoeParams.RL + 2), planningDistance);
smootherBasePath = targetPath.SubPath(targetPath.StartPoint, targetDist);
settings.maxSpeed = GetMaxSpeed(null, LinePath.PointOnPath.Invalid);
settings.endingPositionFixed = true;
settings.endingPositionMax = rndfPathWidth / 2.0;
settings.endingPositionMin = -rndfPathWidth / 2.0;
settings.Options.reverse = true;
Services.UIService.PushLineList(smootherBasePath, curTimestamp, "subpath", true);
Services.UIService.PushLineList(leftBound, curTimestamp, "left bound", true);
Services.UIService.PushLineList(rightBound, curTimestamp, "right bound", true);
SmoothAndTrack(commandLabel, true);
}
public ESK_Bone Clone()
{
return(new ESK_Bone()
{
Name = (string)Name.Clone(),
Index4 = Index4,
RelativeTransform = RelativeTransform.Clone(),
AbsoluteTransform = AbsoluteTransform.Clone(),
ESK_Bones = CloneChildrenRecursive(ESK_Bones),
isSelected = true
});
}
public void TransformPath(CarTimestamp timestamp)
{
// if the current and new timestamp match, ignore the request
if (timestamp != curTimestamp)
{
RelativeTransform relTransform = Services.RelativePose.GetTransform(curTimestamp, timestamp);
// transform the path in place
TransformInPlace(relTransform);
// update the current timestamp
curTimestamp = timestamp;
}
}
internal CAGradientLayer GetLayer(CGSize size)
{
var gradientLayer = new CAGradientLayer();
gradientLayer.Colors = GradientStops.Select(gs => (CGColor)gs.Color).ToArray();
gradientLayer.Locations = GradientStops.Select(gs => new NSNumber(gs.Offset)).ToArray();
var transform = RelativeTransform?.ToNativeTransform(size);
gradientLayer.StartPoint = transform?.TransformPoint(StartPoint) ?? StartPoint;
gradientLayer.EndPoint = transform?.TransformPoint(EndPoint) ?? EndPoint;
return(gradientLayer);
}
protected internal override Shader GetShader(Size size)
{
if (GradientStops.Count == 0)
{
return(null);
}
var colors = GradientStops.SelectToList(s => ((Android.Graphics.Color)GetColorWithOpacity(s.Color)).ToArgb());
var locations = GradientStops.SelectToList(s => (float)s.Offset);
if (GradientStops.Count == 1)
{
// Android LinearGradient requires two ore more stop points.
// We work around this by duplicating the first gradient stop.
colors.Add(colors[0]);
locations.Add(locations[0]);
}
var width = size.Width;
var height = size.Height;
Android.Graphics.Matrix nativeTransformMatrix = null;
if (RelativeTransform != null)
{
var matrix = RelativeTransform.ToMatrix(Point.Zero, new Size(width, height));
matrix.M31 *= (float)width;
matrix.M32 *= (float)height;
nativeTransformMatrix = matrix.ToNative();
}
//Matrix .MapPoints takes an array of floats
var pts = MappingMode == BrushMappingMode.RelativeToBoundingBox
? new[]
{
(float)(StartPoint.X * width),
(float)(StartPoint.Y * height),
(float)(EndPoint.X * width),
(float)(EndPoint.Y * height)
}
: new[]
{
(float)ViewHelper.LogicalToPhysicalPixels(StartPoint.X),
(float)ViewHelper.LogicalToPhysicalPixels(StartPoint.Y),
(float)ViewHelper.LogicalToPhysicalPixels(EndPoint.X),
(float)ViewHelper.LogicalToPhysicalPixels(EndPoint.Y)
};
nativeTransformMatrix?.MapPoints(pts);
nativeTransformMatrix?.Dispose();
return(new LinearGradient(pts[0], pts[1], pts[2], pts[3], colors.ToArray(), locations.ToArray(), Shader.TileMode.Clamp));
}
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);
}
/// <summary>
/// Create matrix to transform image based on relative dimensions of bitmap and drawRect, Stretch mode, and RelativeTransform
/// </summary>
/// <param name="drawRect"></param>
/// <param name="bitmap"></param>
/// <returns></returns>
private Android.Graphics.Matrix GenerateMatrix(Windows.Foundation.Rect drawRect, Bitmap bitmap)
{
var matrix = new Android.Graphics.Matrix();
// Note that bitmap.Width and bitmap.Height (in physical pixels) are automatically scaled up when loaded from local resources, but aren't when acquired externally.
// This means that bitmaps acquired externally might not render the same way on devices with different densities when using Stretch.None.
var sourceRect = new Windows.Foundation.Rect(0, 0, bitmap.Width, bitmap.Height);
var destinationRect = GetArrangedImageRect(sourceRect.Size, drawRect);
matrix.SetRectToRect(sourceRect.ToRectF(), destinationRect.ToRectF(), Android.Graphics.Matrix.ScaleToFit.Fill);
RelativeTransform?.ToNative(matrix, new Size(drawRect.Width, drawRect.Height), isBrush: true);
return(matrix);
}
protected override bool BeginRenderBrushOverride(PrimitiveBuffer primitiveContext, RenderContext renderContext)
{
if (_gradientBrushTexture == null || _refresh)
{
_gradientBrushTexture = BrushCache.Instance.GetGradientBrush(GradientStops);
if (_gradientBrushTexture == null)
{
return(false);
}
}
Matrix finalTransform = renderContext.Transform.Clone();
if (_refresh)
{
_refresh = false;
_effect = ContentManager.Instance.GetEffect(EFFECT_LINEARGRADIENT);
g_startpoint = new float[] { StartPoint.X, StartPoint.Y };
g_endpoint = new float[] { EndPoint.X, EndPoint.Y };
if (MappingMode == BrushMappingMode.Absolute)
{
g_startpoint[0] /= _vertsBounds.Width;
g_startpoint[1] /= _vertsBounds.Height;
g_endpoint[0] /= _vertsBounds.Width;
g_endpoint[1] /= _vertsBounds.Height;
}
g_framesize = new float[] { _vertsBounds.Width, _vertsBounds.Height };
if (RelativeTransform != null)
{
Matrix m = RelativeTransform.GetTransform();
m.Transform(ref g_startpoint[0], ref g_startpoint[1]);
m.Transform(ref g_endpoint[0], ref g_endpoint[1]);
}
}
_effect.Parameters[PARAM_FRAMESIZE] = g_framesize;
_effect.Parameters[PARAM_TRANSFORM] = GetCachedFinalBrushTransform();
_effect.Parameters[PARAM_OPACITY] = (float)(Opacity * renderContext.Opacity);
_effect.Parameters[PARAM_STARTPOINT] = g_startpoint;
_effect.Parameters[PARAM_ENDPOINT] = g_endpoint;
GraphicsDevice.Device.SetSamplerState(0, SamplerState.AddressU, SpreadAddressMode);
_effect.StartRender(_gradientBrushTexture.Texture, finalTransform);
return(true);
}
internal CAGradientLayer GetLayer(CGSize size)
{
var gradientLayer = new CAGradientLayer();
gradientLayer.Colors = GradientStops.Select(gs => (CGColor)gs.Color).ToArray();
gradientLayer.Locations = GradientStops.Select(gs => new NSNumber(gs.Offset)).ToArray();
var transform = RelativeTransform?.ToNativeTransform(size);
#if __IOS__
gradientLayer.StartPoint = transform?.TransformPoint(StartPoint) ?? StartPoint;
gradientLayer.EndPoint = transform?.TransformPoint(EndPoint) ?? EndPoint;
return(gradientLayer);
#elif __MACOS__
throw new NotImplementedException();
#endif
}
public ESK_BoneNonHierarchal Clone()
{
return(new ESK_BoneNonHierarchal()
{
Name = Name,
Index1 = Index1,
Index1_Name = Index1_Name,
Index2 = Index2,
Index2_Name = Index2_Name,
Index3 = Index3,
Index3_Name = Index3_Name,
Index4 = Index4,
AbsoluteTransform = AbsoluteTransform.Clone(),
RelativeTransform = RelativeTransform.Clone()
});
}
private bool CheckFinalExit()
{
// rotate the polygon into the current relative frame
CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;
RelativeTransform relTransform = Services.RelativePose.GetTransform(polygonTimestamp, curTimestamp);
LineSegment finalLine = finalOrientation.Transform(relTransform);
double finalAngle = finalLine.UnitVector.ArcTan;
if (Math.Abs(finalAngle) < 15 * Math.PI / 180.0 && finalLine.ClosestPoint(Coordinates.Zero).Length < 2)
{
// we can just execute a stay in lane
return(true);
}
return(false);
}
protected internal override Shader GetShader(Rect destinationRect)
{
var center = Center;
var radiusX = RadiusX;
var radiusY = RadiusY;
float radius;
if (MappingMode == BrushMappingMode.RelativeToBoundingBox)
{
var size = destinationRect.Size;
center = new Point(center.X * size.Width, Center.Y * size.Height);
radius = (float)(radiusX * size.Width + radiusY * size.Height) / 2.0f; // We take the avg
}
else
{
center = center.LogicalToPhysicalPixels();
radius = ViewHelper.LogicalToPhysicalPixels((radiusX + radiusY) / 2.0d); // We take the avg
}
// Android requires a radius and two or more stop points.
if (radius <= 0 || GradientStops.Count < 2)
{
return(null);
}
var colors = GradientStops.SelectToArray(s => ((Android.Graphics.Color)GetColorWithOpacity(s.Color)).ToArgb());
var locations = GradientStops.SelectToArray(s => (float)s.Offset);
var width = destinationRect.Width;
var height = destinationRect.Height;
var transform = RelativeTransform?.ToNativeMatrix(size: new Windows.Foundation.Size(width, height));
var shader = new RadialGradient(
(float)center.X,
(float)center.Y,
radius,
colors,
locations,
Shader.TileMode.Clamp);
return(shader);
}
public LinePath LinearizeLeftBound(LinearizationOptions options)
{
LinePath bound = FindBoundary(this.width / 2 + options.LaneShiftDistance + 0.3, path);
if (options.Timestamp.IsValid)
{
RelativeTransform relTransform = Services.RelativePose.GetTransform(timestamp, options.Timestamp);
bound.TransformInPlace(relTransform);
}
if (options.EndDistance > options.StartDistance)
{
LinePath.PointOnPath startPoint = bound.AdvancePoint(bound.ZeroPoint, options.StartDistance);
return(bound.SubPath(startPoint, options.EndDistance - options.StartDistance));
}
else
{
return(new LinePath());
}
}
public LinePath LinearizeRightBound(LinearizationOptions options)
{
LinePath bound = rightBound;
if (options.Timestamp.IsValid)
{
RelativeTransform relTransform = Services.RelativePose.GetTransform(timestamp, options.Timestamp);
bound = bound.Transform(relTransform);
}
if (options.EndDistance > options.StartDistance)
{
// TODO: work off centerline path
LinePath.PointOnPath startPoint = bound.AdvancePoint(bound.ZeroPoint, options.StartDistance);
return(bound.SubPath(startPoint, options.EndDistance - options.StartDistance).ShiftLateral(options.LaneShiftDistance));
}
else
{
return(new LinePath());
}
}
/// <summary>
/// Return true if we're approximately aligned with the final lane
/// </summary>
/// <returns></returns>
private UTurnPass CheckInitialAlignment()
{
// rotate the polygon into the current relative frame
CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;
RelativeTransform relTransform = Services.RelativePose.GetTransform(polygonTimestamp, curTimestamp);
LineSegment finalLine = finalOrientation.Transform(relTransform);
double finalAngle = finalLine.UnitVector.ArcTan;
if (finalAngle < 15 * Math.PI / 180.0 && finalAngle > -30 * Math.PI / 180.0)
{
// we can just execute a stay in lane
return(UTurnPass.Final);
}
else if (finalAngle <= -30 * Math.PI / 180.0 && finalAngle >= -90 * Math.PI / 180.0)
{
// want to plan a backward pass, so indicate that we just completed a forward pass
return(UTurnPass.Forward);
}
else
{
return(UTurnPass.Backward);
}
}
protected internal override Shader GetShader(Rect destinationRect)
{
// Android LinearGradient requires two ore more stop points.
if (GradientStops.Count >= 2)
{
var colors = GradientStops.SelectToArray(s => ((Android.Graphics.Color)s.Color).ToArgb());
var locations = GradientStops.SelectToArray(s => (float)s.Offset);
var width = destinationRect.Width;
var height = destinationRect.Height;
var transform =
RelativeTransform?.ToNative(size: new Windows.Foundation.Size(width, height), isBrush: true);
//Matrix .MapPoints takes an array of floats
var pts = MappingMode == BrushMappingMode.RelativeToBoundingBox
? new[]
{
(float)(StartPoint.X * width),
(float)(StartPoint.Y * height),
(float)(EndPoint.X * width),
(float)(EndPoint.Y * height)
}
: new[]
{
(float)StartPoint.X,
(float)StartPoint.Y,
(float)EndPoint.X,
(float)EndPoint.Y
};
transform?.MapPoints(pts);
return(new LinearGradient(pts[0], pts[1], pts[2], pts[3], colors, locations, Shader.TileMode.Clamp));
}
return(null);
}
private ITrackingCommand BuildBackwardPass()
{
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "UTurn Behavior: Initialize Backward Pass");
// rotate the polygon into the current relative frame
CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;
RelativeTransform relTransform = Services.RelativePose.GetTransform(polygonTimestamp, curTimestamp);
relTransform.TransformPointsInPlace(polygon);
finalOrientation = finalOrientation.Transform(relTransform);
polygonTimestamp = curTimestamp;
// retrieve the vehicle state
Coordinates headingVec = new Coordinates(1, 0);
Coordinates headingVec180 = headingVec.Rotate180();
Coordinates headingVec90 = headingVec.Rotate90();
// figure out center point of turn
Circle rearAxleCircle = Circle.FromPointSlopeRadius(new Coordinates(0, 0), headingVec180, minRadius);
Coordinates center = rearAxleCircle.center;
// calculate the points of the wheels
Coordinates rearLeftPoint = headingVec90 * TahoeParams.T / 2;
Coordinates rearRightPoint = -headingVec90 * TahoeParams.T / 2;
Coordinates frontLeftPoint = headingVec * TahoeParams.L + headingVec90 * TahoeParams.T / 2;
Coordinates frontRightPoint = headingVec * TahoeParams.L - headingVec90 * TahoeParams.T / 2;
double minHit = Math.PI / 2.1;
GetMinHitAngle(rearLeftPoint, center, ref minHit);
GetMinHitAngle(rearRightPoint, center, ref minHit);
//GetMinHitAngle(frontLeftPoint, center, false, ref minHit);
//GetMinHitAngle(frontRightPoint, center, false, ref minHit);
frontLeftPoint = headingVec * TahoeParams.FL + headingVec90 * TahoeParams.T / 2;
frontRightPoint = headingVec * TahoeParams.FL - headingVec90 * TahoeParams.T / 2.0;
rearRightPoint = -headingVec * TahoeParams.RL - headingVec90 * TahoeParams.T / 2.0;
rearLeftPoint = -headingVec * TahoeParams.RL + headingVec90 * TahoeParams.T / 2.0;
List <Polygon> obstacles = GetObstacles(curTimestamp);
GetObstacleHitAngle(frontLeftPoint, center, obstacles, ref minHit);
GetObstacleHitAngle(rearLeftPoint, center, obstacles, ref minHit);
GetObstacleHitAngle(rearRightPoint, center, obstacles, ref minHit);
// trim some off the hit for safety'
minHit -= (0.3 / minRadius);
// move at least 0.6 meters
minHit = Math.Max(minHit, 0.6 / minRadius);
// calculate the exit stopping point
// shift the line by the minimum turning radius
Coordinates u = finalOrientation.P1 - finalOrientation.P0;
u = u.Normalize().Rotate90();
Line offsetLine = new Line();
offsetLine.P0 = finalOrientation.P0 + u * (minRadius + 2);
offsetLine.P1 = finalOrientation.P1 + u * (minRadius + 2);
// final the intersection of the current turn circle with a radius of twice the min turn radius and the offset line
Circle twoTurn = new Circle(2 * minRadius + 2, center);
Coordinates[] intersections;
double startAngle = (-center).ArcTan;
if (twoTurn.Intersect(offsetLine, out intersections))
{
// figure out where there were hits
for (int i = 0; i < intersections.Length; i++)
{
// get the angle of the hit
double angle = (intersections[i] - center).ArcTan;
if (angle < startAngle)
{
angle += 2 * Math.PI;
}
angle -= startAngle;
if (angle < minHit)
{
minHit = angle;
}
}
}
minHit = Math.Max(minHit, 0.6 / minRadius);
// set the stopping point at the min hit point
Coordinates stopPoint = rearAxleCircle.GetPoint(startAngle + minHit);
// calculate the stop distance
stopDistance = rearAxleCircle.r * minHit;
stopTimestamp = curTimestamp;
curvature = 1 / minRadius;
// calculate the required steering angle
double steeringCommand = SteeringUtilities.CurvatureToSteeringWheelAngle(-1 / minRadius, uturnSpeed);
ISpeedCommandGenerator shiftSpeedCommand = new ShiftSpeedCommand(TransmissionGear.Reverse);
ISteeringCommandGenerator initialSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, steeringRate, true);
ISpeedCommandGenerator passSpeedCommand = new FeedbackSpeedCommandGenerator(new StopSpeedGenerator(new TravelledDistanceProvider(curTimestamp, stopDistance), uturnSpeed));
ISteeringCommandGenerator passSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, null, false);
ChainedTrackingCommand cmd = new ChainedTrackingCommand(
new TrackingCommand(shiftSpeedCommand, initialSteeringCommand, true),
new TrackingCommand(passSpeedCommand, passSteeringCommand, false));
cmd.Label = backwardLabel;
Services.UIService.PushCircle(new Circle(minRadius, center), curTimestamp, "uturn circle", true);
Services.UIService.PushPoint(stopPoint, curTimestamp, "uturn stop point", true);
Services.UIService.PushPolygon(polygon, curTimestamp, "uturn polygon", true);
return(cmd);
}
private LaneModelTestResult TestLane(LinePath rndfPath, CarTimestamp rndfPathTimestamp, LocalLaneModel laneModel)
{
// construct the result object to hold stuff
LaneModelTestResult result = new LaneModelTestResult();
// project the lane model's path into the rndf path's timestamp
RelativeTransform relTransform = Services.RelativePose.GetTransform(localRoadModel.Timestamp, rndfPathTimestamp);
LinePath laneModelPath = laneModel.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;
// get the heading of the rndf path at its zero point and the heading of the lane model at
// the rndf's zero point
LinePath.PointOnPath laneStartPoint = laneModelPath.GetClosestPoint(rndfZeroPoint.Location);
Coordinates laneModelHeading = laneModelPath.GetSegment(laneStartPoint.Index).UnitVector;
Coordinates rndfHeading = rndfPath.GetSegment(rndfZeroPoint.Index).UnitVector;
double angle = Math.Acos(laneModelHeading.Dot(rndfHeading));
// check if the angle is within limits for comparing offset
if (angle < 30 * Math.PI / 180.0)
{
// get the deviation between lane zero point and rndf zero point
result.rndf_deviation = rndfZeroPoint.Location.DistanceTo(laneZeroPoint.Location);
}
// now start check for how many waypoints are accepted
for (int i = rndfZeroPoint.Index + 1; i < rndfPath.Count; i++)
{
// check the distance along the rndf path
double rndfDistAlong = rndfPath.DistanceBetween(rndfZeroPoint, rndfPath.GetPointOnPath(i));
// break out if we're too far along the rndf
if (rndfDistAlong > 50)
{
break;
}
// get the waypoint
Coordinates waypoint = rndfPath[i];
// project on to lane path
LinePath.PointOnPath laneWaypoint = laneModelPath.GetClosestPoint(waypoint);
// check if we're too far along the lane path
double distAlong = laneModelPath.DistanceBetween(laneZeroPoint, laneWaypoint);
if (distAlong > lane_model_max_dist || distAlong < 0)
{
break;
}
// check if the deviation
double dist = waypoint.DistanceTo(laneWaypoint.Location);
// increment appropriate counts
if (dist < lane_deviation_reject_threshold)
{
result.forward_match_count++;
}
else
{
result.forward_rejection_count++;
}
}
// return the result
return(result);
}
private ILaneModel GetLaneModel(LocalLaneModel laneModel, LinePath rndfPath, double rndfPathWidth, CarTimestamp rndfPathTimestamp)
{
// check the lane model probability
if (laneModel.Probability < lane_probability_reject_threshold)
{
// we're rejecting this, just return a path lane model
return(new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth));
}
// project the lane model's path into the rndf path's timestamp
RelativeTransform relTransform = Services.RelativePose.GetTransform(localRoadModel.Timestamp, rndfPathTimestamp);
LinePath laneModelPath = laneModel.LanePath.Transform(relTransform);
// iterate through the waypoints in the RNDF path and project onto the lane model
// the first one that is over the threshold, we consider the waypoint before as a potential ending point
LinePath.PointOnPath laneModelDeviationEndPoint = new LinePath.PointOnPath();
// flag indicating if any of the waypoint tests failed because of the devation was too high
bool anyDeviationTooHigh = false;
// number of waypoints accepted
int numWaypointsAccepted = 0;
// get the vehicle's position on the rndf path
LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint;
// get the vehicle's position on the lane model
LinePath.PointOnPath laneModelZeroPoint = laneModelPath.ZeroPoint;
// get the last point we want to consider on the lane model
LinePath.PointOnPath laneModelFarthestPoint = laneModelPath.AdvancePoint(laneModelZeroPoint, lane_model_max_dist);
// start walking forward through the waypoints on the rndf path
// this loop will implicitly exit when we're past the end of the lane model as the waypoints
// will stop being close to the lane model (GetClosestPoint returns the end point if we're past the
// end of the path)
for (int i = rndfZeroPoint.Index + 1; i < rndfPath.Count; i++)
{
// get the waypoint
Coordinates rndfWaypoint = rndfPath[i];
// get the closest point on the lane model
LinePath.PointOnPath laneModelClosestPoint = laneModelPath.GetClosestPoint(rndfWaypoint);
// compute the distance between the two
double deviation = rndfWaypoint.DistanceTo(laneModelClosestPoint.Location);
// if this is above the deviation threshold, leave the loop
if (deviation > lane_deviation_reject_threshold || laneModelClosestPoint > laneModelFarthestPoint)
{
// if we're at the end of the lane model path, we don't want to consider this a rejection
if (laneModelClosestPoint < laneModelFarthestPoint)
{
// mark that at least on deviation was too high
anyDeviationTooHigh = true;
}
break;
}
// increment the number of waypoint accepted
numWaypointsAccepted++;
// update the end point of where we're valid as the local road model was OK up to this point
laneModelDeviationEndPoint = laneModelClosestPoint;
}
// go through and figure out how far out the variance is within tolerance
LinePath.PointOnPath laneModelVarianceEndPoint = new LinePath.PointOnPath();
// walk forward from this point until the end of the lane mode path
for (int i = laneModelZeroPoint.Index + 1; i < laneModelPath.Count; i++)
{
// check if we're within the variance toleration
if (laneModel.LaneYVariance[i] <= y_var_reject_threshold)
{
// we are, update the point on path
laneModelVarianceEndPoint = laneModelPath.GetPointOnPath(i);
}
else
{
// we are out of tolerance, break out of the loop
break;
}
}
// now figure out everything out
// determine waypoint rejection status
WaypointRejectionResult waypointRejectionResult;
if (laneModelDeviationEndPoint.Valid)
{
// if the point is valid, that we had at least one waypoint that was ok
// check if any waypoints were rejected
if (anyDeviationTooHigh)
{
// some waypoint was ok, so we know that at least one waypoint was accepted
waypointRejectionResult = WaypointRejectionResult.SomeWaypointsAccepted;
}
else
{
// no waypoint triggered a rejection, but at least one was good
waypointRejectionResult = WaypointRejectionResult.AllWaypointsAccepted;
}
}
else
{
// the point is not valid, so we either had no waypoints or we had all rejections
if (anyDeviationTooHigh)
{
// the first waypoint was rejected, so all are rejected
waypointRejectionResult = WaypointRejectionResult.AllWaypointsRejected;
}
else
{
// the first waypoint (if any) was past the end of the lane model
waypointRejectionResult = WaypointRejectionResult.NoWaypoints;
}
}
// criteria for determining if this path is valid:
// - if some or all waypoints were accepted, than this is probably a good path
// - if some of the waypoints were accepted, we go no farther than the last waypoint that was accepted
// - if there were no waypoints, this is a potentially dangerous situation since we can't reject
// or confirm the local road model. for now, we'll assume that it is correct but this may need to change
// if we start handling intersections in this framework
// - if all waypoints were rejected, than we don't use the local road model
// - go no farther than the laneModelVarianceEndPoint, which is the last point where the y-variance of
// the lane model was in tolerance
// now build out the lane model
ILaneModel finalLaneModel;
// check if we rejected all waypoints or no lane model points satisified the variance threshold
if (waypointRejectionResult == WaypointRejectionResult.AllWaypointsRejected || !laneModelVarianceEndPoint.Valid)
{
// want to just use the path lane model
finalLaneModel = new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth);
}
else
{
// we'll use the lane model
// need to build up the center line as well as left and right bounds
// to build up the center line, use the lane model as far as we feel comfortable (limited by either variance
// or by rejections) and then use the rndf lane after that.
LinePath centerLine = new LinePath();
// figure out the max distance
// if there were no waypoints, set the laneModelDeviationEndPoint to the end of the lane model
if (waypointRejectionResult == WaypointRejectionResult.NoWaypoints)
{
laneModelDeviationEndPoint = laneModelFarthestPoint;
}
// figure out the closer of the end points
LinePath.PointOnPath laneModelEndPoint = (laneModelDeviationEndPoint < laneModelVarianceEndPoint) ? laneModelDeviationEndPoint : laneModelVarianceEndPoint;
bool endAtWaypoint = laneModelEndPoint == laneModelDeviationEndPoint;
// add the lane model to the center line
centerLine.AddRange(laneModelPath.GetSubpathEnumerator(laneModelZeroPoint, laneModelEndPoint));
// create a list to hold the width expansion values
List <double> widthValue = new List <double>();
// make the width expansion values the width of the path plus the 1-sigma values
for (int i = laneModelZeroPoint.Index; i < laneModelZeroPoint.Index + centerLine.Count; i++)
{
widthValue.Add(laneModel.Width / 2.0 + Math.Sqrt(laneModel.LaneYVariance[i]));
}
// now figure out how to add the rndf path
// get the projection of the lane model end point on the rndf path
LinePath.PointOnPath rndfPathStartPoint = rndfPath.GetClosestPoint(laneModelEndPoint.Location);
// if the closest point is past the end of rndf path, then we don't want to tack anything on
if (rndfPathStartPoint != rndfPath.EndPoint)
{
// get the last segment of the new center line
Coordinates centerLineEndSegmentVec = centerLine.EndSegment.UnitVector;
// get the last point of the new center line
Coordinates laneModelEndLoc = laneModelEndPoint.Location;
// now figure out the distance to the next waypoint
LinePath.PointOnPath rndfNextPoint = new LinePath.PointOnPath();
// figure out if we're ending at a waypoint or not
if (endAtWaypoint)
{
rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index + 1);
// if the distance from the start point to the next point is less than rndf_dist_min, then
// use the waypont after
double dist = rndfPath.DistanceBetween(rndfPathStartPoint, rndfNextPoint);
if (dist < rndf_dist_min)
{
if (rndfPathStartPoint.Index < rndfPath.Count - 2)
{
rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index + 2);
}
else if (rndfPath.DistanceBetween(rndfPathStartPoint, rndfPath.EndPoint) < rndf_dist_min)
{
rndfNextPoint = LinePath.PointOnPath.Invalid;
}
else
{
rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, rndf_dist_min * 2);
}
}
}
else
{
// track the last angle we had
double lastAngle = double.NaN;
// walk down the rndf path until we find a valid point
for (double dist = rndf_dist_min; dist <= rndf_dist_max; dist += rndf_dist_step)
{
// advance from the start point by dist
double distTemp = dist;
rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, ref distTemp);
// if the distTemp is > 0, then we're past the end of the path
if (distTemp > 0)
{
// if we're immediately past the end, we don't want to tack anything on
if (dist == rndf_dist_min)
{
rndfNextPoint = LinePath.PointOnPath.Invalid;
}
break;
}
// check the angle made by the last segment of center line and the segment
// formed between the end point of the center line and this new point
double angle = Math.Acos(centerLineEndSegmentVec.Dot((rndfNextPoint.Location - laneModelEndLoc).Normalize()));
// check if the angle satisfies the threshold or we're increasing the angle
if (Math.Abs(angle) < rndf_angle_threshold || (!double.IsNaN(lastAngle) && angle > lastAngle))
{
// break out of the loop, we're done searching
break;
}
lastAngle = angle;
}
}
// tack on the rndf starting at next point going to the end
if (rndfNextPoint.Valid)
{
LinePath subPath = rndfPath.SubPath(rndfNextPoint, rndfPath.EndPoint);
centerLine.AddRange(subPath);
// insert the lane model end point into the sub path
subPath.Insert(0, laneModelEndLoc);
// get the angles
List <Pair <int, double> > angles = subPath.GetIntersectionAngles(0, subPath.Count - 1);
// add the width of the path inflated by the angles
for (int i = 0; i < angles.Count; i++)
{
// calculate the width expansion factor
// 90 deg, 3x width
// 45 deg, 1.5x width
// 0 deg, 1x width
double widthFactor = Math.Pow(angles[i].Right / (Math.PI / 2.0), 2) * 2 + 1;
// add the width value
widthValue.Add(widthFactor * laneModel.Width / 2);
}
// add the final width
widthValue.Add(laneModel.Width / 2);
}
// set the rndf path start point to be the point we used
rndfPathStartPoint = rndfNextPoint;
}
// for now, calculate the left and right bounds the same way we do for the path lane model
// TODO: figure out if we want to do this more intelligently using knowledge of the lane model uncertainty
LinePath leftBound = centerLine.ShiftLateral(widthValue.ToArray());
// get the next shifts
for (int i = 0; i < widthValue.Count; i++)
{
widthValue[i] = -widthValue[i];
}
LinePath rightBound = centerLine.ShiftLateral(widthValue.ToArray());
// build the final lane model
finalLaneModel = new CombinedLaneModel(centerLine, leftBound, rightBound, laneModel.Width, rndfPathTimestamp);
}
SendLaneModelToUI(finalLaneModel, rndfPathTimestamp);
// output the fit result
return(finalLaneModel);
}
protected void RefreshEffectParameters()
{
float w = _vertsBounds.Width;
float h = _vertsBounds.Height;
Vector2 maxuv = TextureMaxUV;
Vector4 brushRect = ViewPort;
// Determine image rect in viewport space
if (Stretch != Stretch.Fill)
{
// Convert brush dimensions to viewport space
Vector2 brushSize = BrushDimensions;
brushSize.X /= w;
brushSize.Y /= h;
switch (Stretch)
{
case Stretch.None:
// Center (or alignment), original size
break;
case Stretch.Uniform:
// Center (or alignment), keep aspect ratio and show borders
{
float ratio = Math.Min(ViewPort.Z / brushSize.X, ViewPort.W / brushSize.Y);
brushSize.X *= ratio;
brushSize.Y *= ratio;
}
break;
case Stretch.UniformToFill:
// Center (or alignment), keep aspect ratio, zoom in to avoid borders
{
float ratio = Math.Max(ViewPort.Z / brushSize.X, ViewPort.W / brushSize.Y);
brushSize.X *= ratio;
brushSize.Y *= ratio;
}
break;
}
// Align brush in viewport
brushRect = AlignBrushInViewport(brushSize);
}
// Compensate for any texture borders
brushRect.Z /= maxuv.X;
brushRect.W /= maxuv.Y;
float repeatx = 1.0f / brushRect.Z;
float repeaty = 1.0f / brushRect.W;
// Transform ViewPort into Texture-space and store for later use in tiling
_textureViewport = new Vector4
{
X = ViewPort.X * repeatx - brushRect.X * repeatx,
Y = ViewPort.Y * repeaty - brushRect.Y * repeaty,
Z = ViewPort.Z * repeatx,
W = ViewPort.W * repeaty
};
// This structure is used for modifying vertex texture coords to position the brush texture
_brushTransform = new Vector4(brushRect.X * repeatx, brushRect.Y * repeaty, repeatx, repeaty);
_relativeTransformCache = RelativeTransform == null ? Matrix.Identity : Matrix.Invert(RelativeTransform.GetTransform());
// Determine if we can use the simpler, more optimised effects
if (Tile == TileMode.None && Stretch != Stretch.UniformToFill)
{
_simplemode = true;
}
else if (ViewPort.X <= 0.0f && ViewPort.Z >= 1.0f && ViewPort.Y <= 0.0f && ViewPort.W >= 1.0f)
{
_simplemode = true;
}
else
{
_simplemode = false;
}
}
protected override bool BeginRenderBrushOverride(PrimitiveBuffer primitiveContext, RenderContext renderContext)
{
if (_gradientBrushTexture == null || _refresh)
{
_gradientBrushTexture = BrushCache.Instance.GetGradientBrush(GradientStops);
if (_gradientBrushTexture == null)
{
return(false);
}
}
Matrix finalTransform = renderContext.Transform.Clone();
if (_refresh)
{
_refresh = false;
_gradientBrushTexture = BrushCache.Instance.GetGradientBrush(GradientStops);
_effect = ContentManager.Instance.GetEffect("radialgradient");
g_focus = new float[] { GradientOrigin.X, GradientOrigin.Y };
g_center = new float[] { Center.X, Center.Y };
g_radius = new float[] { (float)RadiusX, (float)RadiusY };
if (MappingMode == BrushMappingMode.Absolute)
{
g_focus[0] /= _vertsBounds.Width;
g_focus[1] /= _vertsBounds.Height;
g_center[0] /= _vertsBounds.Width;
g_center[1] /= _vertsBounds.Height;
g_radius[0] /= _vertsBounds.Width;
g_radius[1] /= _vertsBounds.Height;
}
g_relativetransform = RelativeTransform == null ? Matrix.Identity : Matrix.Invert(RelativeTransform.GetTransform());
}
_effect.Parameters[PARAM_RELATIVE_TRANSFORM] = g_relativetransform;
_effect.Parameters[PARAM_TRANSFORM] = GetCachedFinalBrushTransform();
_effect.Parameters[PARAM_FOCUS] = g_focus;
_effect.Parameters[PARAM_CENTER] = g_center;
_effect.Parameters[PARAM_RADIUS] = g_radius;
_effect.Parameters[PARAM_OPACITY] = (float)(Opacity * renderContext.Opacity);
GraphicsDevice.Device.SetSamplerState(0, SamplerState.AddressU, SpreadAddressMode);
_effect.StartRender(_gradientBrushTexture.Texture, finalTransform);
return(true);
}
private ITrackingCommand BuildForwardPass(out bool finalPass)
{
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "UTurn Behavior: Determine Forward Pass");
// rotate the polygon into the current relative frame
CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;
RelativeTransform relTransform = Services.RelativePose.GetTransform(polygonTimestamp, curTimestamp);
relTransform.TransformPointsInPlace(polygon);
finalOrientation = finalOrientation.Transform(relTransform);
polygonTimestamp = curTimestamp;
// retrieve the vehicle state
Coordinates headingVec = new Coordinates(1, 0);
Coordinates headingVec90 = headingVec.Rotate90();
// check if we can make it out now
Line curLine = new Line(new Coordinates(0, 0), headingVec);
Coordinates intersectionPoint;
LineSegment finalLine = finalOrientation;
Circle outCircle = Circle.FromLines(curLine, (Line)finalLine, out intersectionPoint);
double steeringCommand;
if (!outCircle.Equals(Circle.Infinite) && outCircle.r > minRadius)
{
// we found an out circle
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "found final pass output");
// build the circle segment
double hitAngle = (intersectionPoint - outCircle.center).ArcTan;
double startAngle = (-outCircle.center).ArcTan;
if (hitAngle < startAngle)
{
hitAngle += 2 * Math.PI;
}
hitAngle -= startAngle;
if (stopOnLine)
{
// get the angle of the end point of the line segment
double endPointAngle = (finalLine.P1 - outCircle.center).ArcTan;
if (endPointAngle < startAngle)
{
endPointAngle += 2 * Math.PI;
}
endPointAngle -= startAngle;
if (endPointAngle < hitAngle)
{
hitAngle = endPointAngle;
}
}
// get the obstacles
Coordinates frontLeftPoint = headingVec * TahoeParams.FL + headingVec90 * TahoeParams.T / 2;
Coordinates frontRightPoint = headingVec * TahoeParams.FL - headingVec90 * TahoeParams.T / 2.0;
Coordinates rearRightPoint = -headingVec * TahoeParams.RL - headingVec90 * TahoeParams.T / 2.0;
List <Polygon> obstacles = GetObstacles(curTimestamp);
GetObstacleHitAngle(frontLeftPoint, outCircle.center, obstacles, ref hitAngle);
GetObstacleHitAngle(frontRightPoint, outCircle.center, obstacles, ref hitAngle);
GetObstacleHitAngle(rearRightPoint, outCircle.center, obstacles, ref hitAngle);
// calculate stopping distance
stopDistance = outCircle.r * hitAngle;
stopTimestamp = curTimestamp;
curvature = 1 / outCircle.r;
intersectionPoint = outCircle.center + Coordinates.FromAngle(startAngle + hitAngle) * outCircle.r;
// calculate steering angle
steeringCommand = SteeringUtilities.CurvatureToSteeringWheelAngle(1 / outCircle.r, uturnSpeed);
// mark that this will be the final pass
finalPass = true;
Services.UIService.PushCircle(outCircle, curTimestamp, "uturn circle", true);
Services.UIService.PushPoint(intersectionPoint, curTimestamp, "uturn stop point", true);
Services.UIService.PushPolygon(polygon, curTimestamp, "uturn polygon", true);
}
else
{
finalPass = false;
// draw out 3 circles
// - front right wheel
// - front left wheel
// - rear left wheel
// figure out center point of turn
Circle rearAxleCircle = Circle.FromPointSlopeRadius(Coordinates.Zero, headingVec, minRadius);
Coordinates center = rearAxleCircle.center;
// calculate the points of the wheels
Coordinates rearLeftPoint = headingVec90 * TahoeParams.T / 2;
Coordinates rearRightPoint = -headingVec90 * TahoeParams.T / 2;
Coordinates frontLeftPoint = headingVec * TahoeParams.L + headingVec90 * TahoeParams.T / 2;
Coordinates frontRightPoint = headingVec * TahoeParams.L - headingVec90 * TahoeParams.T / 2;
// initialize min hit angle to slightly less than 90 degrees
double minHit = Math.PI / 2.1;
//GetMinHitAngle(rearLeftPoint, center, true, ref minHit);
//GetMinHitAngle(rearRightPoint, center, true, ref minHit);
GetMinHitAngle(frontLeftPoint, center, ref minHit);
GetMinHitAngle(frontRightPoint, center, ref minHit);
// get the obstacles
List <Polygon> obstacles = GetObstacles(curTimestamp);
frontLeftPoint = headingVec * TahoeParams.FL + headingVec90 * TahoeParams.T / 2;
frontRightPoint = headingVec * TahoeParams.FL - headingVec90 * TahoeParams.T / 2.0;
rearRightPoint = -headingVec * TahoeParams.RL - headingVec90 * TahoeParams.T / 2.0;
GetObstacleHitAngle(frontLeftPoint, center, obstacles, ref minHit);
GetObstacleHitAngle(frontRightPoint, center, obstacles, ref minHit);
GetObstacleHitAngle(rearRightPoint, center, obstacles, ref minHit);
// trim some off the hit for safety
//if (minHit > 0.5/minRadius)
minHit -= (0.5 / minRadius);
minHit = Math.Max(minHit, 0.6 / minRadius);
double startAngle = (Coordinates.Zero - center).ArcTan;
double hitAngle = startAngle + minHit;
// set the stopping point at the min hit point
Coordinates stopPoint = rearAxleCircle.GetPoint(hitAngle);
// calculate the stop distance
stopDistance = minRadius * minHit;
// calculate the required steering angle
steeringCommand = SteeringUtilities.CurvatureToSteeringWheelAngle(1 / minRadius, uturnSpeed);
Services.UIService.PushCircle(new Circle(minRadius, center), curTimestamp, "uturn circle", true);
Services.UIService.PushPoint(stopPoint, curTimestamp, "uturn stop point", true);
Services.UIService.PushPolygon(polygon, curTimestamp, "uturn polygon", true);
}
// build the command
ISpeedCommandGenerator shiftSpeedCommand = new ShiftSpeedCommand(TransmissionGear.First);
ISteeringCommandGenerator initialSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, steeringRate, true);
ISpeedCommandGenerator passSpeedCommand = new FeedbackSpeedCommandGenerator(new StopSpeedGenerator(new TravelledDistanceProvider(curTimestamp, stopDistance), uturnSpeed));
ISteeringCommandGenerator passSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, null, false);
ChainedTrackingCommand cmd = new ChainedTrackingCommand(
new TrackingCommand(shiftSpeedCommand, initialSteeringCommand, true),
new TrackingCommand(passSpeedCommand, passSteeringCommand, false));
cmd.Label = forwardLabel;
return(cmd);
}
protected override bool BeginRenderOpacityBrushOverride(Texture tex, RenderContext renderContext)
{
if (_gradientBrushTexture == null || _refresh)
{
_gradientBrushTexture = BrushCache.Instance.GetGradientBrush(GradientStops);
if (_gradientBrushTexture == null)
{
return(false);
}
}
Matrix finalTransform = renderContext.Transform.Clone();
if (_refresh)
{
_refresh = false;
_gradientBrushTexture = BrushCache.Instance.GetGradientBrush(GradientStops);
_effect = ContentManager.Instance.GetEffect(EFFECT_RADIALOPACITYGRADIENT);
g_focus = new float[] { GradientOrigin.X, GradientOrigin.Y };
g_center = new float[] { Center.X, Center.Y };
g_radius = new float[] { (float)RadiusX, (float)RadiusY };
if (MappingMode == BrushMappingMode.Absolute)
{
g_focus[0] /= _vertsBounds.Width;
g_focus[1] /= _vertsBounds.Height;
g_center[0] /= _vertsBounds.Width;
g_center[1] /= _vertsBounds.Height;
g_radius[0] /= _vertsBounds.Width;
g_radius[1] /= _vertsBounds.Height;
}
g_relativetransform = RelativeTransform == null ? Matrix.Identity : Matrix.Invert(RelativeTransform.GetTransform());
}
SurfaceDescription desc = tex.GetLevelDescription(0);
float[] g_LowerVertsBounds = new float[] { _vertsBounds.Left / desc.Width, _vertsBounds.Top / desc.Height };
float[] g_UpperVertsBounds = new float[] { _vertsBounds.Right / desc.Width, _vertsBounds.Bottom / desc.Height };
_effect.Parameters[PARAM_RELATIVE_TRANSFORM] = g_relativetransform;
_effect.Parameters[PARAM_TRANSFORM] = GetCachedFinalBrushTransform();
_effect.Parameters[PARAM_FOCUS] = g_focus;
_effect.Parameters[PARAM_CENTER] = g_center;
_effect.Parameters[PARAM_RADIUS] = g_radius;
_effect.Parameters[PARAM_OPACITY] = (float)(Opacity * renderContext.Opacity);
_effect.Parameters[PARAM_ALPHATEX] = _gradientBrushTexture.Texture;
_effect.Parameters[PARAM_UPPERVERTSBOUNDS] = g_UpperVertsBounds;
_effect.Parameters[PARAM_LOWERVERTSBOUNDS] = g_LowerVertsBounds;
GraphicsDevice.Device.SetSamplerState(0, SamplerState.AddressU, SpreadAddressMode);
_effect.StartRender(tex, finalTransform);
return(true);
}
public override void Process(object param)
{
try {
Trace.CorrelationManager.StartLogicalOperation("ChangeLanes");
if (!base.BeginProcess())
{
return;
}
// check if we were given a parameter
if (param != null && param is ChangeLaneBehavior)
{
ChangeLaneBehavior clParam = (ChangeLaneBehavior)param;
HandleBehavior(clParam);
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "got new param -- speed {0}, dist {1}, dist timestamp {2}", clParam.SpeedCommand, clParam.MaxDist, clParam.TimeStamp);
}
// project the lane paths up to the current time
RelativeTransform relTransform = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
LinePath curStartingPath = startingLanePath.Transform(relTransform);
LinePath curEndingPath = endingLanePath.Transform(relTransform);
// get the starting and ending lane models
ILaneModel startingLaneModel, endingLaneModel;
Services.RoadModelProvider.GetLaneChangeModels(curStartingPath, startingLaneWidth, startingNumLanesLeft, startingNumLanesRight,
curEndingPath, endingLaneWidth, changeLeft, behaviorTimestamp, out startingLaneModel, out endingLaneModel);
// calculate the max speed
// TODO: make this look ahead for slowing down
settings.maxSpeed = GetMaxSpeed(endingLanePath, endingLanePath.ZeroPoint);
// get the remaining lane change distance
double remainingDist = GetRemainingLaneChangeDistance();
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "remaining distance is {0}", remainingDist);
if (cancelled)
{
return;
}
// check if we're done
if (remainingDist <= 0)
{
// create a new stay in lane behavior
int deltaLanes = changeLeft ? -1 : 1;
StayInLaneBehavior stayInLane = new StayInLaneBehavior(endingLaneID, speedCommand, null, endingLanePath, endingLaneWidth, startingNumLanesLeft + deltaLanes, startingNumLanesRight - deltaLanes);
stayInLane.TimeStamp = behaviorTimestamp.ts;
Services.BehaviorManager.Execute(stayInLane, null, false);
// send completion report
ForwardCompletionReport(new SuccessCompletionReport(typeof(ChangeLaneBehavior)));
return;
}
// calculate the planning distance
double planningDist = GetPlanningDistance();
if (planningDist < remainingDist + TahoeParams.VL)
{
planningDist = remainingDist + TahoeParams.VL;
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "planning dist {0}", planningDist);
// create a linearization options structure
LinearizationOptions laneOpts = new LinearizationOptions();
laneOpts.Timestamp = curTimestamp;
// get the center line of the target lane starting at the distance we want to enter into it
laneOpts.StartDistance = remainingDist;
laneOpts.EndDistance = planningDist;
LinePath centerLine = endingLaneModel.LinearizeCenterLine(laneOpts);
// add the final center line as a weighting
AddTargetPath(centerLine.Clone(), default_lane_alpha_w);
// pre-pend (0,0) as our position
centerLine.Insert(0, new Coordinates(0, 0));
LinePath leftBound = null;
LinePath rightBound = null;
// figure out if the lane is to the right or left of us
laneOpts.EndDistance = planningDist;
double leftEndingStartDist, rightEndingStartDist;
GetLaneBoundStartDists(curEndingPath, endingLaneWidth, out leftEndingStartDist, out rightEndingStartDist);
if (changeLeft)
{
// we're the target lane is to the left
// get the left bound of the target lane
laneOpts.StartDistance = Math.Max(leftEndingStartDist, TahoeParams.FL);
leftBound = endingLaneModel.LinearizeLeftBound(laneOpts);
}
else
{
// we're changing to the right, get the right bound of the target lane
laneOpts.StartDistance = Math.Max(rightEndingStartDist, TahoeParams.FL);
rightBound = endingLaneModel.LinearizeRightBound(laneOpts);
}
// get the other bound as the starting lane up to 5m before the remaining dist
laneOpts.StartDistance = TahoeParams.FL;
laneOpts.EndDistance = Math.Max(0, remainingDist - 5);
if (changeLeft)
{
if (laneOpts.EndDistance > 0)
{
rightBound = startingLaneModel.LinearizeRightBound(laneOpts);
}
else
{
rightBound = new LinePath();
}
}
else
{
if (laneOpts.EndDistance > 0)
{
leftBound = startingLaneModel.LinearizeLeftBound(laneOpts);
}
else
{
leftBound = new LinePath();
}
}
// append on the that bound of the target lane starting at the remaining dist
laneOpts.StartDistance = Math.Max(remainingDist, TahoeParams.FL);
laneOpts.EndDistance = planningDist;
if (changeLeft)
{
rightBound.AddRange(endingLaneModel.LinearizeRightBound(laneOpts));
}
else
{
leftBound.AddRange(endingLaneModel.LinearizeLeftBound(laneOpts));
}
// set up the planning
smootherBasePath = centerLine;
AddLeftBound(leftBound, !changeLeft);
AddRightBound(rightBound, changeLeft);
Services.UIService.PushLineList(centerLine, curTimestamp, "subpath", true);
Services.UIService.PushLineList(leftBound, curTimestamp, "left bound", true);
Services.UIService.PushLineList(rightBound, curTimestamp, "right bound", true);
if (cancelled)
{
return;
}
// set auxillary options
settings.endingHeading = centerLine.EndSegment.UnitVector.ArcTan;
// smooth and track that stuff
SmoothAndTrack(commandLabel, true);
}
finally {
Trace.CorrelationManager.StopLogicalOperation();
}
}
public void Process(object param)
{
if (cancelled)
{
return;
}
DateTime start = HighResDateTime.Now;
OperationalVehicleState vs = Services.StateProvider.GetVehicleState();
LinePath curBasePath = basePath;
LinePath curLeftBound = leftBound;
LinePath curRightBound = rightBound;
// transform the base path to the current iteration
CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;
if (pathTime != curTimestamp)
{
RelativeTransform relTransform = Services.RelativePose.GetTransform(pathTime, curTimestamp);
curBasePath = curBasePath.Transform(relTransform);
curLeftBound = curLeftBound.Transform(relTransform);
curRightBound = curRightBound.Transform(relTransform);
}
// get the distance between the zero point and the start point
double distToStart = Coordinates.Zero.DistanceTo(curBasePath.GetPoint(curBasePath.StartPoint));
// get the sub-path between 5 and 25 meters ahead
double startDist = TahoeParams.FL;
LinePath.PointOnPath startPoint = curBasePath.AdvancePoint(curBasePath.ZeroPoint, ref startDist);
double endDist = 30;
LinePath.PointOnPath endPoint = curBasePath.AdvancePoint(startPoint, ref endDist);
if (startDist > 0)
{
// we've reached the end
Services.BehaviorManager.Execute(new HoldBrakeBehavior(), null, false);
return;
}
// get the sub-path
LinePath subPath = curBasePath.SubPath(startPoint, endPoint);
// add (0,0) as the starting point
subPath.Insert(0, new Coordinates(0, 0));
// do the same for the left, right bound
startDist = TahoeParams.FL;
endDist = 40;
startPoint = curLeftBound.AdvancePoint(curLeftBound.ZeroPoint, startDist);
endPoint = curLeftBound.AdvancePoint(startPoint, endDist);
curLeftBound = curLeftBound.SubPath(startPoint, endPoint);
startPoint = curRightBound.AdvancePoint(curRightBound.ZeroPoint, startDist);
endPoint = curRightBound.AdvancePoint(startPoint, endDist);
curRightBound = curRightBound.SubPath(startPoint, endPoint);
if (cancelled)
{
return;
}
Services.UIService.PushRelativePath(subPath, curTimestamp, "subpath");
Services.UIService.PushRelativePath(curLeftBound, curTimestamp, "left bound");
Services.UIService.PushRelativePath(curRightBound, curTimestamp, "right bound");
// run a path smoothing iteration
lock (this) {
planner = new PathPlanner();
}
////////////////////////////////////////////////////////////////////////////////////////////////////
// start of obstacle manager - hik
bool obstacleManagerEnable = true;
PathPlanner.SmoothingResult result;
if (obstacleManagerEnable == true)
{
// generate fake obstacles (for simulation testing only)
double obsSize = 10.5 / 2;
List <Coordinates> obstaclePoints = new List <Coordinates>();
List <Obstacle> obstacleClusters = new List <Obstacle>();
// fake left obstacles (for simulation only)
int totalLeftObstacles = Math.Min(0, curLeftBound.Count - 1);
for (int i = 0; i < totalLeftObstacles; i++)
{
obstaclePoints.Clear();
obstaclePoints.Add(curLeftBound[i] + new Coordinates(obsSize, obsSize));
obstaclePoints.Add(curLeftBound[i] + new Coordinates(obsSize, -obsSize));
obstaclePoints.Add(curLeftBound[i] + new Coordinates(-obsSize, -obsSize));
obstaclePoints.Add(curLeftBound[i] + new Coordinates(-obsSize, obsSize));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
}
// fake right obstacles (for simulation only)
int totalRightObstacles = Math.Min(0, curRightBound.Count - 1);
for (int i = 0; i < totalRightObstacles; i++)
{
obstaclePoints.Clear();
obstaclePoints.Add(curRightBound[i] + new Coordinates(obsSize, obsSize));
obstaclePoints.Add(curRightBound[i] + new Coordinates(obsSize, -obsSize));
obstaclePoints.Add(curRightBound[i] + new Coordinates(-obsSize, -obsSize));
obstaclePoints.Add(curRightBound[i] + new Coordinates(-obsSize, obsSize));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
}
// fake center obstacles (for simulation only)
int totalCenterObstacles = Math.Min(0, subPath.Count - 1);
for (int i = 2; i < totalCenterObstacles; i++)
{
obstaclePoints.Clear();
obstaclePoints.Add(subPath[i] + new Coordinates(obsSize, obsSize));
obstaclePoints.Add(subPath[i] + new Coordinates(obsSize, -obsSize));
obstaclePoints.Add(subPath[i] + new Coordinates(-obsSize, -obsSize));
obstaclePoints.Add(subPath[i] + new Coordinates(-obsSize, obsSize));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
}
obstaclePoints.Clear();
obstaclePoints.Add(new Coordinates(10000, 10000));
obstaclePoints.Add(new Coordinates(10000, 10001));
obstaclePoints.Add(new Coordinates(10001, 10000));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
obstaclePoints.Clear();
obstaclePoints.Add(new Coordinates(1000, 1000));
obstaclePoints.Add(new Coordinates(1000, 1001));
obstaclePoints.Add(new Coordinates(1001, 1000));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
obstaclePoints.Clear();
obstaclePoints.Add(new Coordinates(-1000, -1000));
obstaclePoints.Add(new Coordinates(-1000, -1001));
obstaclePoints.Add(new Coordinates(-1001, -1000));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
foreach (Obstacle obs in obstacleClusters)
{
obs.cspacePolygon = new Polygon(obs.obstaclePolygon.points);
}
// find obstacle path and left/right classification
LinePath obstaclePath = new LinePath();
//Boolean successFlag;
//double laneWidthAtPathEnd = 10.0;
//#warning this currently doesn't work
/*obstacleManager.ProcessObstacles(subPath, new LinePath[] { curLeftBound }, new LinePath[] { curRightBound },
* obstacleClusters, laneWidthAtPathEnd,
* out obstaclePath, out successFlag);
*/
// prepare left and right lane bounds
double laneMinSpacing = 0.1;
double laneDesiredSpacing = 0.5;
double laneAlphaS = 10000;
List <Boundary> leftBounds = new List <Boundary>();
List <Boundary> rightBounds = new List <Boundary>();
leftBounds.Add(new Boundary(curLeftBound, laneMinSpacing, laneDesiredSpacing, laneAlphaS));
rightBounds.Add(new Boundary(curRightBound, laneMinSpacing, laneDesiredSpacing, laneAlphaS));
// sort out obstacles as left and right
double obstacleMinSpacing = 0.1;
double obstacleDesiredSpacing = 1.0;
double obstacleAlphaS = 10000;
Boundary bound;
int totalObstacleClusters = obstacleClusters.Count;
for (int i = 0; i < totalObstacleClusters; i++)
{
if (obstacleClusters[i].avoidanceStatus == AvoidanceStatus.Left)
{
// obstacle cluster is to the left of obstacle path
bound = new Boundary(obstacleClusters[i].obstaclePolygon.points, obstacleMinSpacing,
obstacleDesiredSpacing, obstacleAlphaS, true);
bound.CheckFrontBumper = true;
leftBounds.Add(bound);
}
else if (obstacleClusters[i].avoidanceStatus == AvoidanceStatus.Right)
{
// obstacle cluster is to the right of obstacle path
bound = new Boundary(obstacleClusters[i].obstaclePolygon.points, obstacleMinSpacing,
obstacleDesiredSpacing, obstacleAlphaS, true);
bound.CheckFrontBumper = true;
rightBounds.Add(bound);
}
else
{
// obstacle cluster is outside grid, hence ignore obstacle cluster
}
}
Stopwatch stopwatch = new Stopwatch();
stopwatch.Start();
// PlanPath function call with obstacle path and obstacles
result = planner.PlanPath(obstaclePath, obstaclePath, leftBounds, rightBounds,
0, maxSpeed, vs.speed, null, curTimestamp, false);
stopwatch.Stop();
Console.WriteLine("============================================================");
Console.WriteLine("With ObstacleManager - Planner - Elapsed (ms): {0}", stopwatch.ElapsedMilliseconds);
Console.WriteLine("============================================================");
}
else
{
Stopwatch stopwatch = new Stopwatch();
stopwatch.Start();
// original PlanPath function call
result = planner.PlanPath(subPath, curLeftBound, curRightBound,
0, maxSpeed, vs.speed, null, curTimestamp);
stopwatch.Stop();
Console.WriteLine("============================================================");
Console.WriteLine("Without ObstacleManager - Planner - Elapsed (ms): {0}", stopwatch.ElapsedMilliseconds);
Console.WriteLine("============================================================");
}
// end of obstacle manager - hik
////////////////////////////////////////////////////////////////////////////////////////////////////
//PathPlanner.PlanningResult result = planner.PlanPath(subPath, curLeftBound, curRightBound, 0, maxSpeed, vs.speed, null, curTimestamp);
//SmoothedPath path = new SmoothedPath(pathTime);
lock (this) {
planner = null;
}
if (cancelled)
{
return;
}
if (result.result == UrbanChallenge.PathSmoothing.SmoothResult.Sucess)
{
// start tracking the path
Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetSmoothedPathVelocityCommand(result.path));
//Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetConstantSteeringConstantSpeedCommand(-.5, 2));
/*TrackingCommand cmd = new TrackingCommand(
* new FeedbackSpeedCommandGenerator(new ConstantSpeedGenerator(2, null)),
* new SinSteeringCommandGenerator(),
* true);
* Services.TrackingManager.QueueCommand(cmd);*/
// send the path's we're tracking to the UI
Services.UIService.PushRelativePath(result.path, curTimestamp, "smoothed path");
cancelled = true;
}
}
