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