public LinePath LinearizeCenterLine(LinearizationOptions options) { LinePath transformedPath = centerlinePath; if (options.Timestamp.IsValid) { RelativeTransform relTransform = Services.RelativePose.GetTransform(timestamp, options.Timestamp); OperationalTrace.WriteVerbose("in LinearizeCenterLine, tried to find {0}->{1}, got {2}->{3}", timestamp, options.Timestamp, relTransform.OriginTimestamp, relTransform.EndTimestamp); transformedPath = centerlinePath.Transform(relTransform); } LinePath.PointOnPath startPoint = transformedPath.AdvancePoint(centerlinePath.ZeroPoint, options.StartDistance); LinePath subPath = new LinePath(); ; if (options.EndDistance > options.StartDistance) { subPath = transformedPath.SubPath(startPoint, options.EndDistance-options.StartDistance); } if (subPath.Count < 2) { subPath.Clear(); Coordinates startPt = startPoint.Location; subPath.Add(startPt); subPath.Add(centerlinePath.GetSegment(startPoint.Index).UnitVector*Math.Max(options.EndDistance-options.StartDistance, 0.1)+startPt); } return subPath; }
public LinePath LinearizeCenterLine(LinearizationOptions options) { LinePath transformedPath = centerlinePath; if (options.Timestamp.IsValid) { RelativeTransform relTransform = Services.RelativePose.GetTransform(timestamp, options.Timestamp); OperationalTrace.WriteVerbose("in LinearizeCenterLine, tried to find {0}->{1}, got {2}->{3}", timestamp, options.Timestamp, relTransform.OriginTimestamp, relTransform.EndTimestamp); transformedPath = centerlinePath.Transform(relTransform); } LinePath.PointOnPath startPoint = transformedPath.AdvancePoint(centerlinePath.ZeroPoint, options.StartDistance); LinePath subPath = new LinePath();; if (options.EndDistance > options.StartDistance) { subPath = transformedPath.SubPath(startPoint, options.EndDistance - options.StartDistance); } if (subPath.Count < 2) { subPath.Clear(); Coordinates startPt = startPoint.Location; subPath.Add(startPt); subPath.Add(centerlinePath.GetSegment(startPoint.Index).UnitVector *Math.Max(options.EndDistance - options.StartDistance, 0.1) + startPt); } return(subPath); }
public override void Initialize(Behavior b) { base.Initialize(b); Services.ObstaclePipeline.ExtraSpacing = 0; Services.ObstaclePipeline.UseOccupancyGrid = true; // extract the relevant information TurnBehavior cb = (TurnBehavior)b; targetLaneID = cb.TargetLane; // create a fake start lane so we can do the intersection pull stuff pseudoStartLane = new LinePath(); pseudoStartLane.Add(new Coordinates(-1, 0)); pseudoStartLane.Add(Coordinates.Zero); AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(cb.TimeStamp); pseudoStartLane = pseudoStartLane.Transform(absTransform.Invert()); HandleTurnBehavior(cb); curTimestamp = cb.TimeStamp; // do an initial plan without obstacle avoidance DoInitialPlan(); BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "turn behavior - initialized"); }
private LinePath ConvertPath(UrbanChallenge.Common.Path.Path p) { LinePath lineList = new LinePath(); lineList.Add(p[0].Start); for (int i = 0; i < p.Count; i++) { lineList.Add(p[i].End); } return(lineList); }
private LinePath FindBoundary(double offset) { // create a list of shift line segments List <LineSegment> segs = new List <LineSegment>(); foreach (LineSegment ls in basePath.GetSegmentEnumerator()) { segs.Add(ls.ShiftLateral(offset)); } // find the intersection points between all of the segments LinePath boundPoints = new LinePath(); // add the first point boundPoints.Add(segs[0].P0); // loop through the stuff for (int i = 0; i < segs.Count - 1; i++) { // find the intersection Line l0 = (Line)segs[i]; Line l1 = (Line)segs[i + 1]; Coordinates pt; if (l0.Intersect(l1, out pt)) { // figure out the angle of the intersection double angle = Math.Acos(l0.UnitVector.Dot(l1.UnitVector)); // calculate the length factor // 90 deg, 3x width // 0 deg, 2x width double widthFactor = Math.Pow(angle / (Math.PI / 2.0), 2) * 2 + 1; // get the line formed by pt and the corresponding path point Coordinates boundLine = pt - basePath[i + 1]; boundPoints.Add(widthFactor * Math.Abs(offset) * boundLine.Normalize() + basePath[i + 1]); } else { boundPoints.Add(segs[i].P1); } } // add the last point boundPoints.Add(segs[segs.Count - 1].P1); return(boundPoints); }
public static LinePath GetEdgeLine(SideRoadEdge edge) { if (edge == null || !edge.isValid || Math.Abs(edge.curbDistance) < TahoeParams.T/2.0 + 0.1) { return null; } LinePath ret = new LinePath(); Coordinates pt1 = new Coordinates(2, 0).Rotate(edge.curbHeading) + new Coordinates(0, edge.curbDistance); Coordinates pt2 = new Coordinates(10 + TahoeParams.FL, 0).Rotate(edge.curbHeading) + new Coordinates(0, edge.curbDistance); ret.Add(pt1); ret.Add(pt2); return ret; }
public static LinePath GetEdgeLine(SideRoadEdge edge) { if (edge == null || !edge.isValid || Math.Abs(edge.curbDistance) < TahoeParams.T / 2.0 + 0.1) { return(null); } LinePath ret = new LinePath(); Coordinates pt1 = new Coordinates(2, 0).Rotate(edge.curbHeading) + new Coordinates(0, edge.curbDistance); Coordinates pt2 = new Coordinates(10 + TahoeParams.FL, 0).Rotate(edge.curbHeading) + new Coordinates(0, edge.curbDistance); ret.Add(pt1); ret.Add(pt2); return(ret); }
private void ProcessReverse() { double planningDistance = reverseDist - Services.TrackedDistance.GetDistanceTravelled(reverseTimestamp, curTimestamp); // update the rndf path AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp); // get the vehicle relative path LinePath relRecommendedPath = recommendedPath.Transform(absTransform); LinePath targetPath; if (relRecommendedPath.ZeroPoint.Location.Length > 10) { targetPath = new LinePath(); targetPath.Add(new Coordinates(0, 0)); targetPath.Add(new Coordinates(-(planningDistance + TahoeParams.RL + 2), 0)); } else { // get the path in reverse double dist = -(planningDistance + TahoeParams.RL + 2); targetPath = relRecommendedPath.SubPath(relRecommendedPath.ZeroPoint, ref dist); if (dist < 0) { targetPath.Add(relRecommendedPath[0] - relRecommendedPath.GetSegment(0).Vector.Normalize(-dist)); } } AddTargetPath(targetPath, 0.005); avoidanceBasePath = targetPath; double targetDist = Math.Max(targetPath.PathLength - (TahoeParams.RL + 2), planningDistance); smootherBasePath = new LinePath(); smootherBasePath.Add(Coordinates.Zero); smootherBasePath.Add(targetPath.AdvancePoint(targetPath.StartPoint, targetDist).Location); settings.maxSpeed = recommendedSpeed.Speed; settings.Options.reverse = true; settings.Options.w_diff = 3; laneWidthAtPathEnd = 5; useAvoidancePath = false; Services.UIService.PushLineList(smootherBasePath, curTimestamp, "subpath", true); SmoothAndTrack(reverse_label, 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); }
private void HandleTurnBehavior(TurnBehavior cb) { // get the transformer to take us from absolute to relative coordinates AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(cb.TimeStamp); targetPath = cb.TargetLanePath.Transform(absTransform).RemoveZeroLengthSegments(); if (cb.LeftBound != null) { leftBound = new LinePath(cb.LeftBound).Transform(absTransform); } else { leftBound = null; } if (cb.RightBound != null) { rightBound = new LinePath(cb.RightBound).Transform(absTransform); } else { rightBound = null; } if (targetPath.PathLength < 12) { double pathLength = targetPath.PathLength; double ext = 12 - pathLength; targetPath.Add(targetPath.EndPoint.Location + targetPath.EndSegment.Vector.Normalize(ext)); } behaviorTimestamp = absTransform.Timestamp; if (cb.IntersectionPolygon != null) { intersectionPolygon = cb.IntersectionPolygon.Transform(absTransform); // set the polygon to the ui Services.UIService.PushPolygon(cb.IntersectionPolygon, cb.TimeStamp, "intersection polygon", false); } else { intersectionPolygon = null; } this.ignorableObstacles = cb.IgnorableObstacles; Services.ObstaclePipeline.LastIgnoredObstacles = cb.IgnorableObstacles; HandleSpeedCommand(cb.SpeedCommand); }
/// <summary> /// Updates obstacle path /// </summary> /// <param name="obstaclePath">Obstacle path from reduced path</param> private void UpdateObstaclePath(out LinePath obstaclePath) { StartWatch(); // start stopwatch // retrieve reduced path List <Coordinates> reducedPath; gridCost.GetReducedPath(out reducedPath); // prepare obstacle path obstaclePath = new LinePath(); int totalPathPoints = reducedPath.Count; for (int i = 0; i < totalPathPoints; i++) { // convert grid coordinates back to vehicle relative coordinates obstaclePath.Add(new Coordinates((reducedPath[i].X - gridMiddleX) * gridStep, (reducedPath[i].Y - gridMiddleY) * gridStep)); } // display time taken to process obstacle path obstaclePathTime = StopWatch("ObstacleManager - ObstaclePath - "); }
public void GenerateInterconnectPolygon(ArbiterInterconnect ai) { List <Coordinates> polyPoints = new List <Coordinates>(); try { // width double width = 3.0; if (ai.InitialGeneric is ArbiterWaypoint) { ArbiterWaypoint aw = (ArbiterWaypoint)ai.InitialGeneric; width = width < aw.Lane.Width ? aw.Lane.Width : width; } if (ai.FinalGeneric is ArbiterWaypoint) { ArbiterWaypoint aw = (ArbiterWaypoint)ai.FinalGeneric; width = width < aw.Lane.Width ? aw.Lane.Width : width; } if (ai.TurnDirection == ArbiterTurnDirection.UTurn || ai.TurnDirection == ArbiterTurnDirection.Straight || !(ai.InitialGeneric is ArbiterWaypoint) || !(ai.FinalGeneric is ArbiterWaypoint)) { LinePath lp = ai.InterconnectPath.ShiftLateral(width / 2.0); LinePath rp = ai.InterconnectPath.ShiftLateral(-width / 2.0); polyPoints.AddRange(lp); polyPoints.AddRange(rp); ai.TurnPolygon = Polygon.GrahamScan(polyPoints); if (ai.TurnDirection == ArbiterTurnDirection.UTurn) { List <Coordinates> updatedPts = new List <Coordinates>(); LinePath interTmp = ai.InterconnectPath.Clone(); Coordinates pathVec = ai.FinalGeneric.Position - ai.InitialGeneric.Position; interTmp[1] = interTmp[1] + pathVec.Normalize(width / 2.0); interTmp[0] = interTmp[0] - pathVec.Normalize(width / 2.0); lp = interTmp.ShiftLateral(TahoeParams.VL); rp = interTmp.ShiftLateral(-TahoeParams.VL); updatedPts.AddRange(lp); updatedPts.AddRange(rp); ai.TurnPolygon = Polygon.GrahamScan(updatedPts); } } else { // polygon points List <Coordinates> interPoints = new List <Coordinates>(); // waypoint ArbiterWaypoint awI = (ArbiterWaypoint)ai.InitialGeneric; ArbiterWaypoint awF = (ArbiterWaypoint)ai.FinalGeneric; // left and right path LinePath leftPath = new LinePath(); LinePath rightPath = new LinePath(); // some initial points LinePath initialPath = new LinePath(new Coordinates[] { awI.PreviousPartition.Initial.Position, awI.Position }); LinePath il = initialPath.ShiftLateral(width / 2.0); LinePath ir = initialPath.ShiftLateral(-width / 2.0); leftPath.Add(il[1]); rightPath.Add(ir[1]); // some final points LinePath finalPath = new LinePath(new Coordinates[] { awF.Position, awF.NextPartition.Final.Position }); LinePath fl = finalPath.ShiftLateral(width / 2.0); LinePath fr = finalPath.ShiftLateral(-width / 2.0); leftPath.Add(fl[0]); rightPath.Add(fr[0]); // initial and final paths Line iPath = new Line(awI.PreviousPartition.Initial.Position, awI.Position); Line fPath = new Line(awF.Position, awF.NextPartition.Final.Position); // get where the paths intersect and vector to normal path Coordinates c; iPath.Intersect(fPath, out c); Coordinates vector = ai.InterconnectPath.GetClosestPoint(c).Location - c; Coordinates center = c + vector.Normalize((vector.Length / 2.0)); // get width expansion Coordinates iVec = awI.PreviousPartition != null?awI.PreviousPartition.Vector().Normalize(1.0) : awI.NextPartition.Vector().Normalize(1.0); double iRot = -iVec.ArcTan; Coordinates fVec = awF.NextPartition != null?awF.NextPartition.Vector().Normalize(1.0) : awF.PreviousPartition.Vector().Normalize(1.0); fVec = fVec.Rotate(iRot); double fDeg = fVec.ToDegrees(); double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI; double centerWidth = width + width * 2.0 * Math.Abs(arcTan) / 90.0; // get inner point (small scale) Coordinates innerPoint = center + vector.Normalize(centerWidth / 4.0); // get outer Coordinates outerPoint = center - vector.Normalize(centerWidth / 2.0); if (ai.TurnDirection == ArbiterTurnDirection.Right) { rightPath.Insert(1, innerPoint); ai.InnerCoordinates = rightPath; leftPath.Reverse(); leftPath.Insert(1, outerPoint); Polygon p = new Polygon(leftPath.ToArray()); p.AddRange(rightPath.ToArray()); ai.TurnPolygon = p; } else { leftPath.Insert(1, innerPoint); ai.InnerCoordinates = leftPath; rightPath.Reverse(); rightPath.Insert(1, outerPoint); Polygon p = new Polygon(leftPath.ToArray()); p.AddRange(rightPath.ToArray()); ai.TurnPolygon = p; } } } catch (Exception e) { Console.WriteLine("error generating turn polygon: " + ai.ToString()); ai.TurnPolygon = ai.DefaultPoly(); } }
public override void Process(object param) { if (!base.BeginProcess()) { return; } if (param is ZoneTravelingBehavior) { HandleBaseBehavior((ZoneTravelingBehavior)param); } extraObstacles = GetPerimeterObstacles(); if (reverseGear) { ProcessReverse(); return; } AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp); // get the vehicle relative path LinePath relRecommendedPath = recommendedPath.Transform(absTransform); LinePath.PointOnPath zeroPoint = relRecommendedPath.ZeroPoint; // get the distance to the end point double distToEnd = relRecommendedPath.DistanceBetween(zeroPoint, relRecommendedPath.EndPoint); // get the planning distance double planningDist = GetPlanningDistance(); planningDist = Math.Max(planningDist, 20); planningDist -= zeroPoint.Location.Length; if (planningDist < 2.5) planningDist = 2.5; if (distToEnd < planningDist) { // make the speed command at stop speed command behaviorTimestamp = curTimestamp; speedCommand = new StopAtDistSpeedCommand(distToEnd - TahoeParams.FL); planningDist = distToEnd; approachSpeed = recommendedSpeed.Speed; settings.endingHeading = relRecommendedPath.EndSegment.UnitVector.ArcTan; settings.endingPositionFixed = true; settings.endingPositionMax = 2; settings.endingPositionMin = -2; } else { speedCommand = new ScalarSpeedCommand(recommendedSpeed.Speed); } // get the distance of the path segment we care about LinePath pathSegment = relRecommendedPath.SubPath(zeroPoint, planningDist); double avoidanceDist = planningDist + 5; avoidanceBasePath = relRecommendedPath.SubPath(zeroPoint, ref avoidanceDist); if (avoidanceDist > 0) { avoidanceBasePath.Add(avoidanceBasePath.EndPoint.Location + avoidanceBasePath.EndSegment.Vector.Normalize(avoidanceDist)); } // test if we should clear out of arc mode if (arcMode) { if (TestNormalModeClear(relRecommendedPath, zeroPoint)) { prevCurvature = double.NaN; arcMode = false; } } if (Math.Abs(zeroPoint.AlongtrackDistance(Coordinates.Zero)) > 1) { pathSegment.Insert(0, Coordinates.Zero); } else { if (pathSegment[0].DistanceTo(pathSegment[1]) < 1) { pathSegment.RemoveAt(0); } pathSegment[0] = Coordinates.Zero; } if (arcMode) { Coordinates relativeGoalPoint = relRecommendedPath.EndPoint.Location; ArcVoteZone(relativeGoalPoint, extraObstacles); return; } double pathLength = pathSegment.PathLength; if (pathLength < 6) { double additionalDist = 6.25 - pathLength; pathSegment.Add(pathSegment.EndPoint.Location + pathSegment.EndSegment.Vector.Normalize(additionalDist)); } // determine if polygons are to the left or right of the path for (int i = 0; i < zoneBadRegions.Length; i++) { Polygon poly = zoneBadRegions[i].Transform(absTransform); int numLeft = 0; int numRight = 0; foreach (LineSegment ls in pathSegment.GetSegmentEnumerator()) { for (int j = 0; j < poly.Count; j++) { if (ls.IsToLeft(poly[j])) { numLeft++; } else { numRight++; } } } if (numLeft > numRight) { // we'll consider this polygon on the left of the path //additionalLeftBounds.Add(new Boundary(poly, 0.1, 0.1, 0)); } else { //additionalRightBounds.Add(new Boundary(poly, 0.1, 0.1, 0)); } } // TODO: add zone perimeter disablePathAngleCheck = false; laneWidthAtPathEnd = 7; settings.Options.w_diff = 3; smootherBasePath = new LinePath(); smootherBasePath.Add(Coordinates.Zero); smootherBasePath.Add(pathSegment.EndPoint.Location); AddTargetPath(pathSegment, 0.005); settings.maxSpeed = recommendedSpeed.Speed; useAvoidancePath = false; SmoothAndTrack(command_label, true); }
private void ProcessReverse() { double planningDistance = reverseDist - Services.TrackedDistance.GetDistanceTravelled(reverseTimestamp, curTimestamp); // update the rndf path AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp); // get the vehicle relative path LinePath relRecommendedPath = recommendedPath.Transform(absTransform); LinePath targetPath; if (relRecommendedPath.ZeroPoint.Location.Length > 10) { targetPath = new LinePath(); targetPath.Add(new Coordinates(0, 0)); targetPath.Add(new Coordinates(-(planningDistance+TahoeParams.RL+2), 0)); } else { // get the path in reverse double dist = -(planningDistance + TahoeParams.RL + 2); targetPath = relRecommendedPath.SubPath(relRecommendedPath.ZeroPoint, ref dist); if (dist < 0) { targetPath.Add(relRecommendedPath[0] - relRecommendedPath.GetSegment(0).Vector.Normalize(-dist)); } } AddTargetPath(targetPath, 0.005); avoidanceBasePath = targetPath; double targetDist = Math.Max(targetPath.PathLength-(TahoeParams.RL + 2), planningDistance); smootherBasePath = new LinePath(); smootherBasePath.Add(Coordinates.Zero); smootherBasePath.Add(targetPath.AdvancePoint(targetPath.StartPoint, targetDist).Location); settings.maxSpeed = recommendedSpeed.Speed; settings.Options.reverse = true; settings.Options.w_diff = 3; laneWidthAtPathEnd = 5; useAvoidancePath = false; Services.UIService.PushLineList(smootherBasePath, curTimestamp, "subpath", true); SmoothAndTrack(reverse_label, true); }
public override void Process(object param) { if (!base.BeginProcess()) { return; } if (param is ZoneTravelingBehavior) { HandleBaseBehavior((ZoneTravelingBehavior)param); } extraObstacles = GetPerimeterObstacles(); if (reverseGear) { ProcessReverse(); return; } AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp); // get the vehicle relative path LinePath relRecommendedPath = recommendedPath.Transform(absTransform); LinePath.PointOnPath zeroPoint = relRecommendedPath.ZeroPoint; // get the distance to the end point double distToEnd = relRecommendedPath.DistanceBetween(zeroPoint, relRecommendedPath.EndPoint); // get the planning distance double planningDist = GetPlanningDistance(); planningDist = Math.Max(planningDist, 20); planningDist -= zeroPoint.Location.Length; if (planningDist < 2.5) { planningDist = 2.5; } if (distToEnd < planningDist) { // make the speed command at stop speed command behaviorTimestamp = curTimestamp; speedCommand = new StopAtDistSpeedCommand(distToEnd - TahoeParams.FL); planningDist = distToEnd; approachSpeed = recommendedSpeed.Speed; settings.endingHeading = relRecommendedPath.EndSegment.UnitVector.ArcTan; settings.endingPositionFixed = true; settings.endingPositionMax = 2; settings.endingPositionMin = -2; } else { speedCommand = new ScalarSpeedCommand(recommendedSpeed.Speed); } // get the distance of the path segment we care about LinePath pathSegment = relRecommendedPath.SubPath(zeroPoint, planningDist); double avoidanceDist = planningDist + 5; avoidanceBasePath = relRecommendedPath.SubPath(zeroPoint, ref avoidanceDist); if (avoidanceDist > 0) { avoidanceBasePath.Add(avoidanceBasePath.EndPoint.Location + avoidanceBasePath.EndSegment.Vector.Normalize(avoidanceDist)); } // test if we should clear out of arc mode if (arcMode) { if (TestNormalModeClear(relRecommendedPath, zeroPoint)) { prevCurvature = double.NaN; arcMode = false; } } if (Math.Abs(zeroPoint.AlongtrackDistance(Coordinates.Zero)) > 1) { pathSegment.Insert(0, Coordinates.Zero); } else { if (pathSegment[0].DistanceTo(pathSegment[1]) < 1) { pathSegment.RemoveAt(0); } pathSegment[0] = Coordinates.Zero; } if (arcMode) { Coordinates relativeGoalPoint = relRecommendedPath.EndPoint.Location; ArcVoteZone(relativeGoalPoint, extraObstacles); return; } double pathLength = pathSegment.PathLength; if (pathLength < 6) { double additionalDist = 6.25 - pathLength; pathSegment.Add(pathSegment.EndPoint.Location + pathSegment.EndSegment.Vector.Normalize(additionalDist)); } // determine if polygons are to the left or right of the path for (int i = 0; i < zoneBadRegions.Length; i++) { Polygon poly = zoneBadRegions[i].Transform(absTransform); int numLeft = 0; int numRight = 0; foreach (LineSegment ls in pathSegment.GetSegmentEnumerator()) { for (int j = 0; j < poly.Count; j++) { if (ls.IsToLeft(poly[j])) { numLeft++; } else { numRight++; } } } if (numLeft > numRight) { // we'll consider this polygon on the left of the path //additionalLeftBounds.Add(new Boundary(poly, 0.1, 0.1, 0)); } else { //additionalRightBounds.Add(new Boundary(poly, 0.1, 0.1, 0)); } } // TODO: add zone perimeter disablePathAngleCheck = false; laneWidthAtPathEnd = 7; settings.Options.w_diff = 3; smootherBasePath = new LinePath(); smootherBasePath.Add(Coordinates.Zero); smootherBasePath.Add(pathSegment.EndPoint.Location); AddTargetPath(pathSegment, 0.005); settings.maxSpeed = recommendedSpeed.Speed; useAvoidancePath = false; SmoothAndTrack(command_label, true); }
private LinePath ConvertPath(UrbanChallenge.Common.Path.Path p) { LinePath lineList = new LinePath(); lineList.Add(p[0].Start); for (int i = 0; i < p.Count; i++) { lineList.Add(p[i].End); } return lineList; }
protected void GetIntersectionPullPath(LinePath startingPath, LinePath endingPath, Polygon intersectionPolygon, bool addStartingPoint, bool addEndingPoint, LinePath targetPath, ref double pullWeight) { double angle = Math.Acos(startingPath.EndSegment.UnitVector.Dot(endingPath.GetSegment(0).UnitVector)); // get the centroid of the intersection Coordinates centroid; // check if the angle is great than an threshold if (angle > 10*Math.PI/180.0) { // intersect the two lines formed by the starting and ending lanes Line startingLaneLine = new Line(startingPath[startingPath.Count-2], startingPath[startingPath.Count-1]); Line endingLaneLine = new Line(endingPath[1], endingPath[0]); // intersect them stuff and see if the point of intersection is between the two lines Coordinates K; if (!startingLaneLine.Intersect(endingLaneLine, out centroid, out K) || K.X <= 0 || K.Y <= 0) return; } else { // if there is no intersection polygon, there isn't much we can do if (intersectionPolygon == null || intersectionPolygon.Count < 3) { return; } centroid = intersectionPolygon.GetCentroid(); } // calculate the pull weighting dependent on angle of intersection // angle 0 -> 0 weighting // angle 45 -> 0.00025 weighting // angle 90 -> 0.001 weighting pullWeight = Math.Pow(angle/(Math.PI/2), 2)*0.001; // get the relative transform from the behavior timestamp to the current timestamp RelativeTransform transform = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp); centroid = transform.TransformPoint(centroid); if (addStartingPoint) { targetPath.Add(startingPath.EndPoint.Location); } // add the line from exit -> centroid (assuming that exit is already in the target path) targetPath.Add(centroid); if (addEndingPoint) { // add the line from centroid -> entrance targetPath.Add(endingPath[0]); } Services.UIService.PushLineList(targetPath, curTimestamp, "intersection path", true); Services.Dataset.ItemAs<double>("intersection weight").Add(pullWeight, curTimestamp); }
/// <summary> /// Plan in a zone /// </summary> /// <param name="az"></param> /// <param name="goal"></param> /// <returns></returns> public ZonePlan PlanZone(ArbiterZone az, INavigableNode start, INavigableNode goal) { // zone plan ZonePlan zp = new ZonePlan(); zp.Zone = az; zp.Start = start; // given start and goal, get plan double time; List <INavigableNode> nodes; this.Plan(start, goal, out nodes, out time); zp.Time = time; // final path LinePath recommendedPath = new LinePath(); List <INavigableNode> pathNodes = new List <INavigableNode>(); // start and end counts int startCount = 0; int endCount = 0; // check start type if (start is ArbiterParkingSpotWaypoint) { startCount = 2; } // check end type if (goal is ArbiterParkingSpotWaypoint && ((ArbiterParkingSpotWaypoint)goal).ParkingSpot.Zone.Equals(az)) { endCount = -2; } // loop through nodes for (int i = startCount; i < nodes.Count + endCount; i++) { // go to parking spot or endpoint if (nodes[i] is ArbiterParkingSpotWaypoint) { // set zone goal zp.ZoneGoal = ((ArbiterParkingSpotWaypoint)nodes[i]).ParkingSpot.Checkpoint; // set path, return zp.RecommendedPath = recommendedPath; zp.PathNodes = pathNodes; // set route info RouteInformation ri = new RouteInformation(recommendedPath, time, goal.ToString()); CoreCommon.CurrentInformation.Route1 = ri; // return the plan return(zp); } // go to perimeter waypoint if this is one else if (nodes[i] is ArbiterPerimeterWaypoint && ((ArbiterPerimeterWaypoint)nodes[i]).IsExit) { // add recommendedPath.Add(nodes[i].Position); // set zone goal zp.ZoneGoal = nodes[i]; // set path, return zp.RecommendedPath = recommendedPath; zp.PathNodes = pathNodes; // set route info RouteInformation ri = new RouteInformation(recommendedPath, time, goal.ToString()); CoreCommon.CurrentInformation.Route1 = ri; // return the plan return(zp); } // otherwise just add else { // add recommendedPath.Add(nodes[i].Position); pathNodes.Add(nodes[i]); } } // set zone goal zp.ZoneGoal = goal; // set path, return zp.RecommendedPath = recommendedPath; // set route info CoreCommon.CurrentInformation.Route1 = new RouteInformation(recommendedPath, time, goal.ToString()); // return the plan return(zp); }
public static LinePath FindBoundary(double offset, LinePath path) { // create a list of shift line segments List<LineSegment> segs = new List<LineSegment>(); foreach (LineSegment ls in path.GetSegmentEnumerator()) { segs.Add(ls.ShiftLateral(offset)); } // find the intersection points between all of the segments LinePath boundPoints = new LinePath(); // add the first point boundPoints.Add(segs[0].P0); // loop through the stuff for (int i = 0; i < segs.Count-1; i++) { // find the intersection Line l0 = (Line)segs[i]; Line l1 = (Line)segs[i+1]; Coordinates pt; if (l0.Intersect(l1, out pt)) { // figure out the angle of the intersection double angle = Math.Acos(l0.UnitVector.Dot(l1.UnitVector)); double angle2 = Math.Sin(l0.UnitVector.Cross(l1.UnitVector)); // determine if it's a left or right turn bool leftTurn = angle2 > 0; double f = 2.5; if ((leftTurn && offset > 0) || (!leftTurn && offset < 0)) { // left turn and looking for left bound or right turn and looking for right bound f = 0.5; } // calculate the width expansion factor // 90 deg, 3x width // 45 deg, 1.5x width // 0 deg, 1x width double widthFactor = Math.Pow(angle/(Math.PI/2.0),1.25)*f + 1; // get the line formed by pt and the corresponding path point Coordinates boundLine = pt - path[i+1]; boundPoints.Add(widthFactor*Math.Abs(offset)*boundLine.Normalize() + path[i+1]); } else { boundPoints.Add(segs[i].P1); } } // add the last point boundPoints.Add(segs[segs.Count-1].P1); return boundPoints; }
public void GenerateInterconnectPolygon(ArbiterInterconnect ai) { List<Coordinates> polyPoints = new List<Coordinates>(); try { // width double width = 3.0; if (ai.InitialGeneric is ArbiterWaypoint) { ArbiterWaypoint aw = (ArbiterWaypoint)ai.InitialGeneric; width = width < aw.Lane.Width ? aw.Lane.Width : width; } if (ai.FinalGeneric is ArbiterWaypoint) { ArbiterWaypoint aw = (ArbiterWaypoint)ai.FinalGeneric; width = width < aw.Lane.Width ? aw.Lane.Width : width; } if (ai.TurnDirection == ArbiterTurnDirection.UTurn || ai.TurnDirection == ArbiterTurnDirection.Straight || !(ai.InitialGeneric is ArbiterWaypoint) || !(ai.FinalGeneric is ArbiterWaypoint)) { LinePath lp = ai.InterconnectPath.ShiftLateral(width / 2.0); LinePath rp = ai.InterconnectPath.ShiftLateral(-width / 2.0); polyPoints.AddRange(lp); polyPoints.AddRange(rp); ai.TurnPolygon = Polygon.GrahamScan(polyPoints); if (ai.TurnDirection == ArbiterTurnDirection.UTurn) { List<Coordinates> updatedPts = new List<Coordinates>(); LinePath interTmp = ai.InterconnectPath.Clone(); Coordinates pathVec = ai.FinalGeneric.Position - ai.InitialGeneric.Position; interTmp[1] = interTmp[1] + pathVec.Normalize(width / 2.0); interTmp[0] = interTmp[0] - pathVec.Normalize(width / 2.0); lp = interTmp.ShiftLateral(TahoeParams.VL); rp = interTmp.ShiftLateral(-TahoeParams.VL); updatedPts.AddRange(lp); updatedPts.AddRange(rp); ai.TurnPolygon = Polygon.GrahamScan(updatedPts); } } else { // polygon points List<Coordinates> interPoints = new List<Coordinates>(); // waypoint ArbiterWaypoint awI = (ArbiterWaypoint)ai.InitialGeneric; ArbiterWaypoint awF = (ArbiterWaypoint)ai.FinalGeneric; // left and right path LinePath leftPath = new LinePath(); LinePath rightPath = new LinePath(); // some initial points LinePath initialPath = new LinePath(new Coordinates[] { awI.PreviousPartition.Initial.Position, awI.Position }); LinePath il = initialPath.ShiftLateral(width / 2.0); LinePath ir = initialPath.ShiftLateral(-width / 2.0); leftPath.Add(il[1]); rightPath.Add(ir[1]); // some final points LinePath finalPath = new LinePath(new Coordinates[] { awF.Position, awF.NextPartition.Final.Position }); LinePath fl = finalPath.ShiftLateral(width / 2.0); LinePath fr = finalPath.ShiftLateral(-width / 2.0); leftPath.Add(fl[0]); rightPath.Add(fr[0]); // initial and final paths Line iPath = new Line(awI.PreviousPartition.Initial.Position, awI.Position); Line fPath = new Line(awF.Position, awF.NextPartition.Final.Position); // get where the paths intersect and vector to normal path Coordinates c; iPath.Intersect(fPath, out c); Coordinates vector = ai.InterconnectPath.GetClosestPoint(c).Location - c; Coordinates center = c + vector.Normalize((vector.Length / 2.0)); // get width expansion Coordinates iVec = awI.PreviousPartition != null ? awI.PreviousPartition.Vector().Normalize(1.0) : awI.NextPartition.Vector().Normalize(1.0); double iRot = -iVec.ArcTan; Coordinates fVec = awF.NextPartition != null ? awF.NextPartition.Vector().Normalize(1.0) : awF.PreviousPartition.Vector().Normalize(1.0); fVec = fVec.Rotate(iRot); double fDeg = fVec.ToDegrees(); double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI; double centerWidth = width + width * 2.0 * Math.Abs(arcTan) / 90.0; // get inner point (small scale) Coordinates innerPoint = center + vector.Normalize(centerWidth / 4.0); // get outer Coordinates outerPoint = center - vector.Normalize(centerWidth / 2.0); if (ai.TurnDirection == ArbiterTurnDirection.Right) { rightPath.Insert(1, innerPoint); ai.InnerCoordinates = rightPath; leftPath.Reverse(); leftPath.Insert(1, outerPoint); Polygon p = new Polygon(leftPath.ToArray()); p.AddRange(rightPath.ToArray()); ai.TurnPolygon = p; } else { leftPath.Insert(1, innerPoint); ai.InnerCoordinates = leftPath; rightPath.Reverse(); rightPath.Insert(1, outerPoint); Polygon p = new Polygon(leftPath.ToArray()); p.AddRange(rightPath.ToArray()); ai.TurnPolygon = p; } } } catch (Exception e) { Console.WriteLine("error generating turn polygon: " + ai.ToString()); ai.TurnPolygon = ai.DefaultPoly(); } }
private PlanningPhase PlanMidTurn(LinePath curStartingLane, LinePath curEndingLane) { // get the distance to the first point of the ending lane double remainingDistance = Coordinates.Zero.DistanceTo(curEndingLane[0]); // get the desired planning distance double planningDistance = GetPlanningDistance(); // calculate the distance of the target lane that we want double targetLaneDist = Math.Max(planningDistance - remainingDistance, 5); // get the lane model ILaneModel endingLaneModel = Services.RoadModelProvider.GetLaneModel(curEndingLane, endingLaneWidth, curTimestamp, endingLanesLeft, endingLanesRight); // linearize the lane model LinePath centerLine, leftBound, rightBound; LinearizeStayInLane(endingLaneModel, targetLaneDist, 0, null, 2, 2, curTimestamp, out centerLine, out leftBound, out rightBound); // create a copy LinePath endingCenterLine = centerLine.Clone(); // add as a target path AddTargetPath(endingCenterLine, 0.001); // add the lane bounds AddLeftBound(leftBound, false); AddRightBound(rightBound, false); // insert our position into the center line path centerLine.Insert(0, Coordinates.Zero); // get the intersection pull path LinePath intersectionPullPath = new LinePath(); double pullWeighting = 0; GetIntersectionPullPath(curStartingLane, endingCenterLine, intersectionPolygon, true, true, intersectionPullPath, ref pullWeighting); if (intersectionPullPath.Count > 0) { // add as a weighting term AddTargetPath(intersectionPullPath, pullWeighting); } BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "in stay in lane, center line count: {0}, left bound count: {1}, right bound count: {2}", centerLine.Count, leftBound.Count, rightBound.Count); Services.UIService.PushLineList(centerLine, curTimestamp, "subpath", true); Services.UIService.PushLineList(leftBound, curTimestamp, "left bound", true); Services.UIService.PushLineList(rightBound, curTimestamp, "right bound", true); // calculate max speed for the path over the next 120 meters double advanceDist = speed_path_advance_dist - remainingDistance; LinePath combinedPath = new LinePath(); combinedPath.Add(Coordinates.Zero); if (advanceDist > 0) { // add the ending lane with the appropriate distance combinedPath.AddRange(curEndingLane.GetSubpathEnumerator(0, curEndingLane.AdvancePoint(curEndingLane.StartPoint, advanceDist).Index + 1)); } else { // add the first segment of the ending lane combinedPath.Add(curEndingLane[0]); combinedPath.Add(curEndingLane[1]); } settings.maxSpeed = GetMaxSpeed(combinedPath, combinedPath.StartPoint); settings.maxEndingSpeed = GetMaxSpeed(combinedPath, combinedPath.AdvancePoint(combinedPath.StartPoint, planningDistance)); BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "max speed set to {0}", settings.maxSpeed); if (remainingDistance >= 15) { // get the ending lane heading //settings.endingHeading = centerLine.EndSegment.UnitVector.ArcTan; } // set the avoidance path to be the previously smoothed path updated to the current time AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp); avoidanceBasePath = turnBasePath.Transform(absTransform); avoidanceBasePath = avoidanceBasePath.SubPath(avoidanceBasePath.ZeroPoint, avoidanceBasePath.EndPoint); //avoidanceBasePath = null; smootherBasePath = avoidanceBasePath.Clone(); maxSmootherBasePathAdvancePoint = smootherBasePath.GetClosestPoint(endingCenterLine.StartPoint.Location); //useAvoidancePath = true; SmoothAndTrack(commandLabel, true); if (cancelled) return PlanningPhase.MidTurn; // check if the remaining distance on the starting lane is less than some small tolerance. // if it is, switch to the mid-turn phase if (remainingDistance < 6) { return PlanningPhase.EndingLane; } else { return PlanningPhase.MidTurn; } }
public static LinePath FindBoundary(double offset, LinePath path) { // create a list of shift line segments List <LineSegment> segs = new List <LineSegment>(); foreach (LineSegment ls in path.GetSegmentEnumerator()) { segs.Add(ls.ShiftLateral(offset)); } // find the intersection points between all of the segments LinePath boundPoints = new LinePath(); // add the first point boundPoints.Add(segs[0].P0); // loop through the stuff for (int i = 0; i < segs.Count - 1; i++) { // find the intersection Line l0 = (Line)segs[i]; Line l1 = (Line)segs[i + 1]; Coordinates pt; if (l0.Intersect(l1, out pt)) { // figure out the angle of the intersection double angle = Math.Acos(l0.UnitVector.Dot(l1.UnitVector)); double angle2 = Math.Sin(l0.UnitVector.Cross(l1.UnitVector)); // determine if it's a left or right turn bool leftTurn = angle2 > 0; double f = 2.5; if ((leftTurn && offset > 0) || (!leftTurn && offset < 0)) { // left turn and looking for left bound or right turn and looking for right bound f = 0.5; } // calculate the width expansion factor // 90 deg, 3x width // 45 deg, 1.5x width // 0 deg, 1x width double widthFactor = Math.Pow(angle / (Math.PI / 2.0), 1.25) * f + 1; // get the line formed by pt and the corresponding path point Coordinates boundLine = pt - path[i + 1]; boundPoints.Add(widthFactor * Math.Abs(offset) * boundLine.Normalize() + path[i + 1]); } else { boundPoints.Add(segs[i].P1); } } // add the last point boundPoints.Add(segs[segs.Count - 1].P1); return(boundPoints); }
/// <summary> /// Updates obstacle path /// </summary> /// <param name="obstaclePath">Obstacle path from reduced path</param> private void UpdateObstaclePath(out LinePath obstaclePath) { StartWatch(); // start stopwatch // retrieve reduced path List<Coordinates> reducedPath; gridCost.GetReducedPath(out reducedPath); // prepare obstacle path obstaclePath = new LinePath(); int totalPathPoints = reducedPath.Count; for (int i = 0; i < totalPathPoints; i++) { // convert grid coordinates back to vehicle relative coordinates obstaclePath.Add(new Coordinates((reducedPath[i].X - gridMiddleX) * gridStep, (reducedPath[i].Y - gridMiddleY) * gridStep)); } // display time taken to process obstacle path obstaclePathTime = StopWatch("ObstacleManager - ObstaclePath - "); }
private LinePath BuildPath(ArbiterLanePartition parition, bool forward, double distBackTarget, double distForwardTarget) { if (forward) { // iterate all the way backward until we have our distance ArbiterLanePartition backPartition = parition; double distBack = 0; while (backPartition.Initial.PreviousPartition != null && distBack < distBackTarget) { backPartition = backPartition.Initial.PreviousPartition; distBack += backPartition.Length; } LinePath path = new LinePath(); // insert the backmost point while (backPartition != parition) { path.Add(backPartition.Initial.Position); backPartition = backPartition.Final.NextPartition; } // add our initial position path.Add(parition.Initial.Position); // add our final position path.Add(parition.Final.Position); double distForward = 0; while (parition.Final.NextPartition != null && distForward < distForwardTarget) { parition = parition.Final.NextPartition; distForward += parition.Length; path.Add(parition.Final.Position); } return path; } else { // iterate all the way backward until we have our distance ArbiterLanePartition backPartition = parition; double distBack = 0; while (backPartition.Final.NextPartition != null && distBack < distBackTarget) { backPartition = backPartition.Final.NextPartition; distBack += backPartition.Length; } LinePath path = new LinePath(); // insert the backmost point while (backPartition != parition) { path.Add(backPartition.Final.Position); backPartition = backPartition.Initial.PreviousPartition; } // add our initial position path.Add(parition.Final.Position); // add our final position path.Add(parition.Initial.Position); double distForward = 0; while (parition.Initial.PreviousPartition != null && distForward < distForwardTarget) { parition = parition.Initial.PreviousPartition; distForward += parition.Length; path.Add(parition.Initial.Position); } return path; } }
private LinePath FindBoundary(double offset) { // create a list of shift line segments List<LineSegment> segs = new List<LineSegment>(); foreach (LineSegment ls in basePath.GetSegmentEnumerator()) { segs.Add(ls.ShiftLateral(offset)); } // find the intersection points between all of the segments LinePath boundPoints = new LinePath(); // add the first point boundPoints.Add(segs[0].P0); // loop through the stuff for (int i = 0; i < segs.Count-1; i++) { // find the intersection Line l0 = (Line)segs[i]; Line l1 = (Line)segs[i+1]; Coordinates pt; if (l0.Intersect(l1, out pt)) { // figure out the angle of the intersection double angle = Math.Acos(l0.UnitVector.Dot(l1.UnitVector)); // calculate the length factor // 90 deg, 3x width // 0 deg, 2x width double widthFactor = Math.Pow(angle/(Math.PI/2.0),2)*2 + 1; // get the line formed by pt and the corresponding path point Coordinates boundLine = pt - basePath[i+1]; boundPoints.Add(widthFactor*Math.Abs(offset)*boundLine.Normalize() + basePath[i+1]); } else { boundPoints.Add(segs[i].P1); } } // add the last point boundPoints.Add(segs[segs.Count-1].P1); return boundPoints; }
/// <summary> /// Check if we can go /// </summary> /// <param name="vs"></param> private bool CanGo(VehicleState vs) { #region Moving Vehicles Inside Turn // check if we can still go through this turn if (TacticalDirector.VehicleAreas.ContainsKey(this.turn)) { // get the subpath of the interconnect we care about LinePath.PointOnPath frontPos = this.turn.InterconnectPath.GetClosestPoint(vs.Front); LinePath aiSubpath = this.turn.InterconnectPath.SubPath(frontPos, this.turn.InterconnectPath.EndPoint); if (aiSubpath.PathLength > 4.0) { aiSubpath = aiSubpath.SubPath(aiSubpath.StartPoint, aiSubpath.PathLength - 2.0); // get vehicles List <VehicleAgent> turnVehicles = TacticalDirector.VehicleAreas[this.turn]; // loop vehicles foreach (VehicleAgent va in turnVehicles) { // check if inside turn LinePath.PointOnPath vaPop = aiSubpath.GetClosestPoint(va.ClosestPosition); if (!va.IsStopped && this.turn.TurnPolygon.IsInside(va.ClosestPosition) && !vaPop.Equals(aiSubpath.StartPoint) && !vaPop.Equals(aiSubpath.EndPoint)) { ArbiterOutput.Output("Vehicle seen inside our turn: " + va.ToString() + ", stopping"); return(false); } } } } #endregion // test if this turn is part of an intersection if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(this.turn.InitialGeneric.AreaSubtypeWaypointId)) { // intersection ArbiterIntersection inter = CoreCommon.RoadNetwork.IntersectionLookup[this.turn.InitialGeneric.AreaSubtypeWaypointId]; // check if priority lanes exist for this interconnect if (inter.PriorityLanes.ContainsKey(this.turn)) { // get all the default priority lanes List <IntersectionInvolved> priorities = inter.PriorityLanes[this.turn]; // get the subpath of the interconnect we care about //LinePath.PointOnPath frontPos = this.turn.InterconnectPath.GetClosestPoint(vs.Front); LinePath aiSubpath = new LinePath(new List <Coordinates>(new Coordinates[] { vs.Front, this.turn.FinalGeneric.Position })); //this.turn.InterconnectPath.SubPath(frontPos, this.turn.InterconnectPath.EndPoint); // check if path ended if (aiSubpath.Count < 2) { return(true); } // determine all of the new priority lanes List <IntersectionInvolved> updatedPriorities = new List <IntersectionInvolved>(); #region Determine new priority areas given position // loop through old priorities foreach (IntersectionInvolved ii in priorities) { // check ii lane if (ii.Area is ArbiterLane) { #region Lane Intersects Turn Path w/ Point of No Return // check if the waypoint is not the last waypoint in the lane if (ii.Exit == null || ((ArbiterWaypoint)ii.Exit).NextPartition != null) { // check where line intersects path Coordinates?intersect = this.LaneIntersectsPath(ii, aiSubpath, this.turn.FinalGeneric); // check for an intersection if (intersect.HasValue) { // distance to intersection double distanceToIntersection = (intersect.Value.DistanceTo(vs.Front) + ((ArbiterLane)ii.Area).LanePath().GetClosestPoint(vs.Front).Location.DistanceTo(vs.Front)) / 2.0; // determine if we can stop before or after the intersection double distanceToStoppage = RoadToolkit.DistanceToStop(CoreCommon.Communications.GetVehicleSpeed().Value); // check dist to intersection > distance to stoppage if (distanceToIntersection > distanceToStoppage) { // add updated priority updatedPriorities.Add(new IntersectionInvolved(new ArbiterWaypoint(intersect.Value, new ArbiterWaypointId(0, ((ArbiterLane)ii.Area).LaneId)), ii.Area, ArbiterTurnDirection.Straight)); } else { ArbiterOutput.Output("Passed point of No Return for Lane: " + ii.Area.ToString()); } } } #endregion // we know there is an exit and it is the last waypoint of the segment, fuxk! else { #region Turns Intersect // get point to look at if exists ArbiterInterconnect interconnect; Coordinates? intersect = this.TurnIntersects(aiSubpath, ii.Exit, out interconnect); // check for the intersect if (intersect.HasValue) { ArbiterLane al = (ArbiterLane)ii.Area; LinePath lp = al.LanePath().SubPath(al.LanePath().StartPoint, al.LanePath().GetClosestPoint(ii.Exit.Position)); lp.Add(interconnect.InterconnectPath.EndPoint.Location); // get our time to the intersection point //double ourTime = Math.Min(4.0, Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.001 ? aiSubpath.PathLength / 1.0 : aiSubpath.PathLength / Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value)); // get our time to the intersection point double ourSpeed = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value); double stoppedTime = ourSpeed < 1.0 ? 1.5 : 0.0; double extraTime = 1.5; double interconnectTime = aiSubpath.PathLength / this.turn.MaximumDefaultSpeed; double ourTime = Math.Min(6.5, stoppedTime + extraTime + interconnectTime); // get closest vehicle in that lane to the intersection List <VehicleAgent> toLook = new List <VehicleAgent>(); if (TacticalDirector.VehicleAreas.ContainsKey(ii.Area)) { foreach (VehicleAgent tmpVa in TacticalDirector.VehicleAreas[ii.Area]) { double upstreamDist = al.DistanceBetween(tmpVa.ClosestPosition, ii.Exit.Position); if (upstreamDist > 0 && tmpVa.PassedDelayedBirth) { toLook.Add(tmpVa); } } } if (TacticalDirector.VehicleAreas.ContainsKey(interconnect)) { toLook.AddRange(TacticalDirector.VehicleAreas[interconnect]); } // check length of stuff to look at if (toLook.Count > 0) { foreach (VehicleAgent va in toLook) { // distance along path to location of intersect double distToIntersect = lp.DistanceBetween(lp.GetClosestPoint(va.ClosestPosition), lp.GetClosestPoint(aiSubpath.GetClosestPoint(va.ClosestPosition).Location)); double speed = va.Speed == 0.0 ? 0.01 : va.Speed; double vaTime = distToIntersect / Math.Abs(speed); if (vaTime > 0 && vaTime < ourTime) { ArbiterOutput.Output("va: " + va.ToString() + " CollisionTimer: " + vaTime.ToString("f2") + " < TimeUs: " + ourTime.ToString("F2") + ", NOGO"); return(false); } } } } #endregion } } } #endregion #region Updated Priority Intersection Code // loop through updated priorities bool updatedPrioritiesClear = true; foreach (IntersectionInvolved ii in updatedPriorities) { // lane ArbiterLane al = (ArbiterLane)ii.Area; // get our time to the intersection point double ourSpeed = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value); double stoppedTime = ourSpeed < 1.0 ? 1.5 : 0.0; double extraTime = 1.5; double interconnectTime = aiSubpath.PathLength / this.turn.MaximumDefaultSpeed; double ourTime = Math.Min(6.5, stoppedTime + extraTime + interconnectTime); // double outTime = Math.Min(4.0, Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.001 ? aiSubpath.PathLength / 1.0 : aiSubpath.PathLength / Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value)); // get closest vehicle in that lane to the intersection if (TacticalDirector.VehicleAreas.ContainsKey(ii.Area)) { // get lane vehicles List <VehicleAgent> vas = TacticalDirector.VehicleAreas[ii.Area]; // determine for all VehicleAgent closestLaneVa = null; double closestDistanceVa = Double.MaxValue; double closestTime = Double.MaxValue; foreach (VehicleAgent testVa in vas) { // check upstream double distance = al.DistanceBetween(testVa.ClosestPosition, ii.Exit.Position); // get speed double speed = testVa.Speed; double time = testVa.StateMonitor.Observed.speedValid ? distance / Math.Abs(speed) : distance / al.Way.Segment.SpeedLimits.MaximumSpeed; // check distance > 0 if (distance > 0) { // check if closer or none other exists if (closestLaneVa == null || time < closestTime) { closestLaneVa = testVa; closestDistanceVa = distance; closestTime = time; } } } // check if closest exists if (closestLaneVa != null) { // set va VehicleAgent va = closestLaneVa; double distance = closestDistanceVa; // check dist and birth time if (distance > 0 && va.PassedDelayedBirth) { // check time double speed = va.Speed == 0.0 ? 0.01 : va.Speed; double time = va.StateMonitor.Observed.speedValid ? distance / Math.Abs(speed) : distance / al.Way.Segment.SpeedLimits.MaximumSpeed; // too close if (!al.LanePolygon.IsInside(CoreCommon.Communications.GetVehicleState().Front) && distance < 25 && (!va.StateMonitor.Observed.speedValid || !va.StateMonitor.Observed.isStopped) && CoreCommon.Communications.GetVehicleState().Front.DistanceTo(va.ClosestPosition) < 20) { ArbiterOutput.Output("Turn, NOGO, Lane: " + al.ToString() + " vehicle: " + va.ToString() + " possibly moving to close, stopping"); //return false; updatedPrioritiesClear = false; return(false); } else if (time > 0 && time < ourTime) { ArbiterOutput.Output("Turn, NOGO, Lane: " + al.ToString() + ", va: " + va.ToString() + ", stopped: " + va.IsStopped.ToString() + ", timeUs: " + ourTime.ToString("f2") + ", timeThem: " + time.ToString("f2")); //return false; updatedPrioritiesClear = false; return(false); } else { ArbiterOutput.Output("Turn, CANGO, Lane: " + al.ToString() + ", va: " + va.ToString() + ", stopped: " + va.IsStopped.ToString() + ", timeUs: " + ourTime.ToString("f2") + ", timeThem: " + time.ToString("f2")); //return true; } } } else { ArbiterOutput.Output("Turn, CANGO, Lane: " + al.ToString() + " has no traffic vehicles"); } } } return(updatedPrioritiesClear); #endregion } } // fall through fine to go ArbiterOutput.Output("In Turn, CAN GO, Clear of vehicles upstream"); return(true); }
/// <summary> /// Plan in a zone /// </summary> /// <param name="az"></param> /// <param name="goal"></param> /// <returns></returns> public ZonePlan PlanZone(ArbiterZone az, INavigableNode start, INavigableNode goal) { // zone plan ZonePlan zp = new ZonePlan(); zp.Zone = az; zp.Start = start; // given start and goal, get plan double time; List<INavigableNode> nodes; this.Plan(start, goal, out nodes, out time); zp.Time = time; // final path LinePath recommendedPath = new LinePath(); List<INavigableNode> pathNodes = new List<INavigableNode>(); // start and end counts int startCount = 0; int endCount = 0; // check start type if(start is ArbiterParkingSpotWaypoint) startCount = 2; // check end type if(goal is ArbiterParkingSpotWaypoint && ((ArbiterParkingSpotWaypoint)goal).ParkingSpot.Zone.Equals(az)) endCount = -2; // loop through nodes for(int i = startCount; i < nodes.Count + endCount; i++) { // go to parking spot or endpoint if(nodes[i] is ArbiterParkingSpotWaypoint) { // set zone goal zp.ZoneGoal = ((ArbiterParkingSpotWaypoint)nodes[i]).ParkingSpot.Checkpoint; // set path, return zp.RecommendedPath = recommendedPath; zp.PathNodes = pathNodes; // set route info RouteInformation ri = new RouteInformation(recommendedPath, time, goal.ToString()); CoreCommon.CurrentInformation.Route1 = ri; // return the plan return zp; } // go to perimeter waypoint if this is one else if(nodes[i] is ArbiterPerimeterWaypoint && ((ArbiterPerimeterWaypoint)nodes[i]).IsExit) { // add recommendedPath.Add(nodes[i].Position); // set zone goal zp.ZoneGoal = nodes[i]; // set path, return zp.RecommendedPath = recommendedPath; zp.PathNodes = pathNodes; // set route info RouteInformation ri = new RouteInformation(recommendedPath, time, goal.ToString()); CoreCommon.CurrentInformation.Route1 = ri; // return the plan return zp; } // otherwise just add else { // add recommendedPath.Add(nodes[i].Position); pathNodes.Add(nodes[i]); } } // set zone goal zp.ZoneGoal = goal; // set path, return zp.RecommendedPath = recommendedPath; // set route info CoreCommon.CurrentInformation.Route1 = new RouteInformation(recommendedPath, time, goal.ToString()); // return the plan return zp; }
private LinePath BuildPath(ArbiterLanePartition parition, bool forward, double distBackTarget, double distForwardTarget) { if (forward) { // iterate all the way backward until we have our distance ArbiterLanePartition backPartition = parition; double distBack = 0; while (backPartition.Initial.PreviousPartition != null && distBack < distBackTarget) { backPartition = backPartition.Initial.PreviousPartition; distBack += backPartition.Length; } LinePath path = new LinePath(); // insert the backmost point while (backPartition != parition) { path.Add(backPartition.Initial.Position); backPartition = backPartition.Final.NextPartition; } // add our initial position path.Add(parition.Initial.Position); // add our final position path.Add(parition.Final.Position); double distForward = 0; while (parition.Final.NextPartition != null && distForward < distForwardTarget) { parition = parition.Final.NextPartition; distForward += parition.Length; path.Add(parition.Final.Position); } return(path); } else { // iterate all the way backward until we have our distance ArbiterLanePartition backPartition = parition; double distBack = 0; while (backPartition.Final.NextPartition != null && distBack < distBackTarget) { backPartition = backPartition.Final.NextPartition; distBack += backPartition.Length; } LinePath path = new LinePath(); // insert the backmost point while (backPartition != parition) { path.Add(backPartition.Final.Position); backPartition = backPartition.Initial.PreviousPartition; } // add our initial position path.Add(parition.Final.Position); // add our final position path.Add(parition.Initial.Position); double distForward = 0; while (parition.Initial.PreviousPartition != null && distForward < distForwardTarget) { parition = parition.Initial.PreviousPartition; distForward += parition.Length; path.Add(parition.Initial.Position); } return(path); } }