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; } }