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);
}
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 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 LinePath LinearizeLeftBound(LinearizationOptions options)
{
LinePath bound = FindBoundary(this.width / 2 + options.LaneShiftDistance + 0.3, path);
if (options.Timestamp.IsValid)
{
RelativeTransform relTransform = Services.RelativePose.GetTransform(timestamp, options.Timestamp);
bound.TransformInPlace(relTransform);
}
if (options.EndDistance > options.StartDistance)
{
LinePath.PointOnPath startPoint = bound.AdvancePoint(bound.ZeroPoint, options.StartDistance);
return(bound.SubPath(startPoint, options.EndDistance - options.StartDistance));
}
else
{
return(new LinePath());
}
}
public LinePath LinearizeRightBound(LinearizationOptions options)
{
LinePath bound = rightBound;
if (options.Timestamp.IsValid)
{
RelativeTransform relTransform = Services.RelativePose.GetTransform(timestamp, options.Timestamp);
bound = bound.Transform(relTransform);
}
if (options.EndDistance > options.StartDistance)
{
// TODO: work off centerline path
LinePath.PointOnPath startPoint = bound.AdvancePoint(bound.ZeroPoint, options.StartDistance);
return(bound.SubPath(startPoint, options.EndDistance - options.StartDistance).ShiftLateral(options.LaneShiftDistance));
}
else
{
return(new LinePath());
}
}
public void Process(object param)
{
if (cancelled)
{
return;
}
DateTime start = HighResDateTime.Now;
OperationalVehicleState vs = Services.StateProvider.GetVehicleState();
LinePath curBasePath = basePath;
LinePath curLeftBound = leftBound;
LinePath curRightBound = rightBound;
// transform the base path to the current iteration
CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;
if (pathTime != curTimestamp)
{
RelativeTransform relTransform = Services.RelativePose.GetTransform(pathTime, curTimestamp);
curBasePath = curBasePath.Transform(relTransform);
curLeftBound = curLeftBound.Transform(relTransform);
curRightBound = curRightBound.Transform(relTransform);
}
// get the distance between the zero point and the start point
double distToStart = Coordinates.Zero.DistanceTo(curBasePath.GetPoint(curBasePath.StartPoint));
// get the sub-path between 5 and 25 meters ahead
double startDist = TahoeParams.FL;
LinePath.PointOnPath startPoint = curBasePath.AdvancePoint(curBasePath.ZeroPoint, ref startDist);
double endDist = 30;
LinePath.PointOnPath endPoint = curBasePath.AdvancePoint(startPoint, ref endDist);
if (startDist > 0)
{
// we've reached the end
Services.BehaviorManager.Execute(new HoldBrakeBehavior(), null, false);
return;
}
// get the sub-path
LinePath subPath = curBasePath.SubPath(startPoint, endPoint);
// add (0,0) as the starting point
subPath.Insert(0, new Coordinates(0, 0));
// do the same for the left, right bound
startDist = TahoeParams.FL;
endDist = 40;
startPoint = curLeftBound.AdvancePoint(curLeftBound.ZeroPoint, startDist);
endPoint = curLeftBound.AdvancePoint(startPoint, endDist);
curLeftBound = curLeftBound.SubPath(startPoint, endPoint);
startPoint = curRightBound.AdvancePoint(curRightBound.ZeroPoint, startDist);
endPoint = curRightBound.AdvancePoint(startPoint, endDist);
curRightBound = curRightBound.SubPath(startPoint, endPoint);
if (cancelled)
{
return;
}
Services.UIService.PushRelativePath(subPath, curTimestamp, "subpath");
Services.UIService.PushRelativePath(curLeftBound, curTimestamp, "left bound");
Services.UIService.PushRelativePath(curRightBound, curTimestamp, "right bound");
// run a path smoothing iteration
lock (this) {
planner = new PathPlanner();
}
////////////////////////////////////////////////////////////////////////////////////////////////////
// start of obstacle manager - hik
bool obstacleManagerEnable = true;
PathPlanner.SmoothingResult result;
if (obstacleManagerEnable == true)
{
// generate fake obstacles (for simulation testing only)
double obsSize = 10.5 / 2;
List <Coordinates> obstaclePoints = new List <Coordinates>();
List <Obstacle> obstacleClusters = new List <Obstacle>();
// fake left obstacles (for simulation only)
int totalLeftObstacles = Math.Min(0, curLeftBound.Count - 1);
for (int i = 0; i < totalLeftObstacles; i++)
{
obstaclePoints.Clear();
obstaclePoints.Add(curLeftBound[i] + new Coordinates(obsSize, obsSize));
obstaclePoints.Add(curLeftBound[i] + new Coordinates(obsSize, -obsSize));
obstaclePoints.Add(curLeftBound[i] + new Coordinates(-obsSize, -obsSize));
obstaclePoints.Add(curLeftBound[i] + new Coordinates(-obsSize, obsSize));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
}
// fake right obstacles (for simulation only)
int totalRightObstacles = Math.Min(0, curRightBound.Count - 1);
for (int i = 0; i < totalRightObstacles; i++)
{
obstaclePoints.Clear();
obstaclePoints.Add(curRightBound[i] + new Coordinates(obsSize, obsSize));
obstaclePoints.Add(curRightBound[i] + new Coordinates(obsSize, -obsSize));
obstaclePoints.Add(curRightBound[i] + new Coordinates(-obsSize, -obsSize));
obstaclePoints.Add(curRightBound[i] + new Coordinates(-obsSize, obsSize));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
}
// fake center obstacles (for simulation only)
int totalCenterObstacles = Math.Min(0, subPath.Count - 1);
for (int i = 2; i < totalCenterObstacles; i++)
{
obstaclePoints.Clear();
obstaclePoints.Add(subPath[i] + new Coordinates(obsSize, obsSize));
obstaclePoints.Add(subPath[i] + new Coordinates(obsSize, -obsSize));
obstaclePoints.Add(subPath[i] + new Coordinates(-obsSize, -obsSize));
obstaclePoints.Add(subPath[i] + new Coordinates(-obsSize, obsSize));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
}
obstaclePoints.Clear();
obstaclePoints.Add(new Coordinates(10000, 10000));
obstaclePoints.Add(new Coordinates(10000, 10001));
obstaclePoints.Add(new Coordinates(10001, 10000));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
obstaclePoints.Clear();
obstaclePoints.Add(new Coordinates(1000, 1000));
obstaclePoints.Add(new Coordinates(1000, 1001));
obstaclePoints.Add(new Coordinates(1001, 1000));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
obstaclePoints.Clear();
obstaclePoints.Add(new Coordinates(-1000, -1000));
obstaclePoints.Add(new Coordinates(-1000, -1001));
obstaclePoints.Add(new Coordinates(-1001, -1000));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
foreach (Obstacle obs in obstacleClusters)
{
obs.cspacePolygon = new Polygon(obs.obstaclePolygon.points);
}
// find obstacle path and left/right classification
LinePath obstaclePath = new LinePath();
//Boolean successFlag;
//double laneWidthAtPathEnd = 10.0;
//#warning this currently doesn't work
/*obstacleManager.ProcessObstacles(subPath, new LinePath[] { curLeftBound }, new LinePath[] { curRightBound },
* obstacleClusters, laneWidthAtPathEnd,
* out obstaclePath, out successFlag);
*/
// prepare left and right lane bounds
double laneMinSpacing = 0.1;
double laneDesiredSpacing = 0.5;
double laneAlphaS = 10000;
List <Boundary> leftBounds = new List <Boundary>();
List <Boundary> rightBounds = new List <Boundary>();
leftBounds.Add(new Boundary(curLeftBound, laneMinSpacing, laneDesiredSpacing, laneAlphaS));
rightBounds.Add(new Boundary(curRightBound, laneMinSpacing, laneDesiredSpacing, laneAlphaS));
// sort out obstacles as left and right
double obstacleMinSpacing = 0.1;
double obstacleDesiredSpacing = 1.0;
double obstacleAlphaS = 10000;
Boundary bound;
int totalObstacleClusters = obstacleClusters.Count;
for (int i = 0; i < totalObstacleClusters; i++)
{
if (obstacleClusters[i].avoidanceStatus == AvoidanceStatus.Left)
{
// obstacle cluster is to the left of obstacle path
bound = new Boundary(obstacleClusters[i].obstaclePolygon.points, obstacleMinSpacing,
obstacleDesiredSpacing, obstacleAlphaS, true);
bound.CheckFrontBumper = true;
leftBounds.Add(bound);
}
else if (obstacleClusters[i].avoidanceStatus == AvoidanceStatus.Right)
{
// obstacle cluster is to the right of obstacle path
bound = new Boundary(obstacleClusters[i].obstaclePolygon.points, obstacleMinSpacing,
obstacleDesiredSpacing, obstacleAlphaS, true);
bound.CheckFrontBumper = true;
rightBounds.Add(bound);
}
else
{
// obstacle cluster is outside grid, hence ignore obstacle cluster
}
}
Stopwatch stopwatch = new Stopwatch();
stopwatch.Start();
// PlanPath function call with obstacle path and obstacles
result = planner.PlanPath(obstaclePath, obstaclePath, leftBounds, rightBounds,
0, maxSpeed, vs.speed, null, curTimestamp, false);
stopwatch.Stop();
Console.WriteLine("============================================================");
Console.WriteLine("With ObstacleManager - Planner - Elapsed (ms): {0}", stopwatch.ElapsedMilliseconds);
Console.WriteLine("============================================================");
}
else
{
Stopwatch stopwatch = new Stopwatch();
stopwatch.Start();
// original PlanPath function call
result = planner.PlanPath(subPath, curLeftBound, curRightBound,
0, maxSpeed, vs.speed, null, curTimestamp);
stopwatch.Stop();
Console.WriteLine("============================================================");
Console.WriteLine("Without ObstacleManager - Planner - Elapsed (ms): {0}", stopwatch.ElapsedMilliseconds);
Console.WriteLine("============================================================");
}
// end of obstacle manager - hik
////////////////////////////////////////////////////////////////////////////////////////////////////
//PathPlanner.PlanningResult result = planner.PlanPath(subPath, curLeftBound, curRightBound, 0, maxSpeed, vs.speed, null, curTimestamp);
//SmoothedPath path = new SmoothedPath(pathTime);
lock (this) {
planner = null;
}
if (cancelled)
{
return;
}
if (result.result == UrbanChallenge.PathSmoothing.SmoothResult.Sucess)
{
// start tracking the path
Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetSmoothedPathVelocityCommand(result.path));
//Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetConstantSteeringConstantSpeedCommand(-.5, 2));
/*TrackingCommand cmd = new TrackingCommand(
* new FeedbackSpeedCommandGenerator(new ConstantSpeedGenerator(2, null)),
* new SinSteeringCommandGenerator(),
* true);
* Services.TrackingManager.QueueCommand(cmd);*/
// send the path's we're tracking to the UI
Services.UIService.PushRelativePath(result.path, curTimestamp, "smoothed path");
cancelled = true;
}
}
public static Polygon PartitionPolygon(ArbiterLanePartition alp)
{
if (alp.Initial.PreviousPartition != null &&
alp.Final.NextPartition != null &&
alp.Length < 30.0 && alp.Length > 4.0)
{
// get partition turn direction
ArbiterTurnDirection pTD = PartitionTurnDirection(alp);
// check if angles of previous and next are such that not straight through
if (pTD != ArbiterTurnDirection.Straight)
{
// get partition poly
ArbiterInterconnect tmpAi = alp.ToInterconnect;
tmpAi.TurnDirection = pTD;
GenerateInterconnectPolygon(tmpAi);
Polygon pPoly = tmpAi.TurnPolygon;
// here is default partition polygon
LinePath alplb = alp.PartitionPath.ShiftLateral(-alp.Lane.Width / 2.0);
LinePath alprb = alp.PartitionPath.ShiftLateral(alp.Lane.Width / 2.0);
alprb.Reverse();
List <Coordinates> alpdefaultPoly = alplb;
alpdefaultPoly.AddRange(alprb);
// get full poly
pPoly.AddRange(alpdefaultPoly);
pPoly = Polygon.GrahamScan(pPoly);
return(pPoly);
}
}
else if (alp.Length >= 30)
{
Polygon pBase = GenerateSimplePartitionPolygon(alp, alp.PartitionPath, alp.Lane.Width);
if (alp.Initial.PreviousPartition != null && Math.Abs(FinalIntersectionAngle(alp.Initial.PreviousPartition)) > 15)
{
// initial portion
Coordinates i1 = alp.Initial.Position - alp.Initial.PreviousPartition.Vector().Normalize(15.0);
Coordinates i2 = alp.Initial.Position;
Coordinates i3 = i2 + alp.Vector().Normalize(15.0);
LinePath il12 = new LinePath(new Coordinates[] { i1, i2 });
LinePath il23 = new LinePath(new Coordinates[] { i2, i3 });
LinePath il13 = new LinePath(new Coordinates[] { i1, i3 });
Coordinates iCC = il13.GetClosestPoint(i2).Location;
if (GeneralToolkit.TriangleArea(i1, i2, i3) < 0)
{
il13 = il13.ShiftLateral(iCC.DistanceTo(i2) + alp.Lane.Width / 2.0);
}
else
{
il13 = il13.ShiftLateral(-iCC.DistanceTo(i2) + alp.Lane.Width / 2.0);
}
LinePath.PointOnPath iCCP = il13.GetClosestPoint(iCC);
iCCP = il13.AdvancePoint(iCCP, -10.0);
il13 = il13.SubPath(iCCP, 20.0);
Polygon iBase = GenerateSimplePolygon(il23, alp.Lane.Width);
iBase.Add(il13[1]);
Polygon iP = Polygon.GrahamScan(iBase);
pBase = PolygonToolkit.PolygonUnion(new List <Polygon>(new Polygon[] { pBase, iP }));
}
if (alp.Final.NextPartition != null && Math.Abs(FinalIntersectionAngle(alp)) > 15)
{
// initial portion
Coordinates i1 = alp.Final.Position - alp.Vector().Normalize(15.0);
Coordinates i2 = alp.Final.Position;
Coordinates i3 = i2 + alp.Final.NextPartition.Vector().Normalize(15.0);
LinePath il12 = new LinePath(new Coordinates[] { i1, i2 });
LinePath il23 = new LinePath(new Coordinates[] { i2, i3 });
LinePath il13 = new LinePath(new Coordinates[] { i1, i3 });
Coordinates iCC = il13.GetClosestPoint(i2).Location;
if (GeneralToolkit.TriangleArea(i1, i2, i3) < 0)
{
il13 = il13.ShiftLateral(iCC.DistanceTo(i2) + alp.Lane.Width / 2.0);
}
else
{
il13 = il13.ShiftLateral(-iCC.DistanceTo(i2) + alp.Lane.Width / 2.0);
}
LinePath.PointOnPath iCCP = il13.GetClosestPoint(iCC);
iCCP = il13.AdvancePoint(iCCP, -10.0);
il13 = il13.SubPath(iCCP, 20.0);
Polygon iBase = GenerateSimplePolygon(il12, alp.Lane.Width);
iBase.Add(il13[0]);
Polygon iP = Polygon.GrahamScan(iBase);
pBase = PolygonToolkit.PolygonUnion(new List <Polygon>(new Polygon[] { pBase, iP }));
}
return(pBase);
}
// fall out
return(null);
}
private ILaneModel GetLaneModel(LocalLaneModel laneModel, LinePath rndfPath, double rndfPathWidth, CarTimestamp rndfPathTimestamp)
{
// check the lane model probability
if (laneModel.Probability < lane_probability_reject_threshold)
{
// we're rejecting this, just return a path lane model
return(new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth));
}
// project the lane model's path into the rndf path's timestamp
RelativeTransform relTransform = Services.RelativePose.GetTransform(localRoadModel.Timestamp, rndfPathTimestamp);
LinePath laneModelPath = laneModel.LanePath.Transform(relTransform);
// iterate through the waypoints in the RNDF path and project onto the lane model
// the first one that is over the threshold, we consider the waypoint before as a potential ending point
LinePath.PointOnPath laneModelDeviationEndPoint = new LinePath.PointOnPath();
// flag indicating if any of the waypoint tests failed because of the devation was too high
bool anyDeviationTooHigh = false;
// number of waypoints accepted
int numWaypointsAccepted = 0;
// get the vehicle's position on the rndf path
LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint;
// get the vehicle's position on the lane model
LinePath.PointOnPath laneModelZeroPoint = laneModelPath.ZeroPoint;
// get the last point we want to consider on the lane model
LinePath.PointOnPath laneModelFarthestPoint = laneModelPath.AdvancePoint(laneModelZeroPoint, lane_model_max_dist);
// start walking forward through the waypoints on the rndf path
// this loop will implicitly exit when we're past the end of the lane model as the waypoints
// will stop being close to the lane model (GetClosestPoint returns the end point if we're past the
// end of the path)
for (int i = rndfZeroPoint.Index + 1; i < rndfPath.Count; i++)
{
// get the waypoint
Coordinates rndfWaypoint = rndfPath[i];
// get the closest point on the lane model
LinePath.PointOnPath laneModelClosestPoint = laneModelPath.GetClosestPoint(rndfWaypoint);
// compute the distance between the two
double deviation = rndfWaypoint.DistanceTo(laneModelClosestPoint.Location);
// if this is above the deviation threshold, leave the loop
if (deviation > lane_deviation_reject_threshold || laneModelClosestPoint > laneModelFarthestPoint)
{
// if we're at the end of the lane model path, we don't want to consider this a rejection
if (laneModelClosestPoint < laneModelFarthestPoint)
{
// mark that at least on deviation was too high
anyDeviationTooHigh = true;
}
break;
}
// increment the number of waypoint accepted
numWaypointsAccepted++;
// update the end point of where we're valid as the local road model was OK up to this point
laneModelDeviationEndPoint = laneModelClosestPoint;
}
// go through and figure out how far out the variance is within tolerance
LinePath.PointOnPath laneModelVarianceEndPoint = new LinePath.PointOnPath();
// walk forward from this point until the end of the lane mode path
for (int i = laneModelZeroPoint.Index + 1; i < laneModelPath.Count; i++)
{
// check if we're within the variance toleration
if (laneModel.LaneYVariance[i] <= y_var_reject_threshold)
{
// we are, update the point on path
laneModelVarianceEndPoint = laneModelPath.GetPointOnPath(i);
}
else
{
// we are out of tolerance, break out of the loop
break;
}
}
// now figure out everything out
// determine waypoint rejection status
WaypointRejectionResult waypointRejectionResult;
if (laneModelDeviationEndPoint.Valid)
{
// if the point is valid, that we had at least one waypoint that was ok
// check if any waypoints were rejected
if (anyDeviationTooHigh)
{
// some waypoint was ok, so we know that at least one waypoint was accepted
waypointRejectionResult = WaypointRejectionResult.SomeWaypointsAccepted;
}
else
{
// no waypoint triggered a rejection, but at least one was good
waypointRejectionResult = WaypointRejectionResult.AllWaypointsAccepted;
}
}
else
{
// the point is not valid, so we either had no waypoints or we had all rejections
if (anyDeviationTooHigh)
{
// the first waypoint was rejected, so all are rejected
waypointRejectionResult = WaypointRejectionResult.AllWaypointsRejected;
}
else
{
// the first waypoint (if any) was past the end of the lane model
waypointRejectionResult = WaypointRejectionResult.NoWaypoints;
}
}
// criteria for determining if this path is valid:
// - if some or all waypoints were accepted, than this is probably a good path
// - if some of the waypoints were accepted, we go no farther than the last waypoint that was accepted
// - if there were no waypoints, this is a potentially dangerous situation since we can't reject
// or confirm the local road model. for now, we'll assume that it is correct but this may need to change
// if we start handling intersections in this framework
// - if all waypoints were rejected, than we don't use the local road model
// - go no farther than the laneModelVarianceEndPoint, which is the last point where the y-variance of
// the lane model was in tolerance
// now build out the lane model
ILaneModel finalLaneModel;
// check if we rejected all waypoints or no lane model points satisified the variance threshold
if (waypointRejectionResult == WaypointRejectionResult.AllWaypointsRejected || !laneModelVarianceEndPoint.Valid)
{
// want to just use the path lane model
finalLaneModel = new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth);
}
else
{
// we'll use the lane model
// need to build up the center line as well as left and right bounds
// to build up the center line, use the lane model as far as we feel comfortable (limited by either variance
// or by rejections) and then use the rndf lane after that.
LinePath centerLine = new LinePath();
// figure out the max distance
// if there were no waypoints, set the laneModelDeviationEndPoint to the end of the lane model
if (waypointRejectionResult == WaypointRejectionResult.NoWaypoints)
{
laneModelDeviationEndPoint = laneModelFarthestPoint;
}
// figure out the closer of the end points
LinePath.PointOnPath laneModelEndPoint = (laneModelDeviationEndPoint < laneModelVarianceEndPoint) ? laneModelDeviationEndPoint : laneModelVarianceEndPoint;
bool endAtWaypoint = laneModelEndPoint == laneModelDeviationEndPoint;
// add the lane model to the center line
centerLine.AddRange(laneModelPath.GetSubpathEnumerator(laneModelZeroPoint, laneModelEndPoint));
// create a list to hold the width expansion values
List <double> widthValue = new List <double>();
// make the width expansion values the width of the path plus the 1-sigma values
for (int i = laneModelZeroPoint.Index; i < laneModelZeroPoint.Index + centerLine.Count; i++)
{
widthValue.Add(laneModel.Width / 2.0 + Math.Sqrt(laneModel.LaneYVariance[i]));
}
// now figure out how to add the rndf path
// get the projection of the lane model end point on the rndf path
LinePath.PointOnPath rndfPathStartPoint = rndfPath.GetClosestPoint(laneModelEndPoint.Location);
// if the closest point is past the end of rndf path, then we don't want to tack anything on
if (rndfPathStartPoint != rndfPath.EndPoint)
{
// get the last segment of the new center line
Coordinates centerLineEndSegmentVec = centerLine.EndSegment.UnitVector;
// get the last point of the new center line
Coordinates laneModelEndLoc = laneModelEndPoint.Location;
// now figure out the distance to the next waypoint
LinePath.PointOnPath rndfNextPoint = new LinePath.PointOnPath();
// figure out if we're ending at a waypoint or not
if (endAtWaypoint)
{
rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index + 1);
// if the distance from the start point to the next point is less than rndf_dist_min, then
// use the waypont after
double dist = rndfPath.DistanceBetween(rndfPathStartPoint, rndfNextPoint);
if (dist < rndf_dist_min)
{
if (rndfPathStartPoint.Index < rndfPath.Count - 2)
{
rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index + 2);
}
else if (rndfPath.DistanceBetween(rndfPathStartPoint, rndfPath.EndPoint) < rndf_dist_min)
{
rndfNextPoint = LinePath.PointOnPath.Invalid;
}
else
{
rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, rndf_dist_min * 2);
}
}
}
else
{
// track the last angle we had
double lastAngle = double.NaN;
// walk down the rndf path until we find a valid point
for (double dist = rndf_dist_min; dist <= rndf_dist_max; dist += rndf_dist_step)
{
// advance from the start point by dist
double distTemp = dist;
rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, ref distTemp);
// if the distTemp is > 0, then we're past the end of the path
if (distTemp > 0)
{
// if we're immediately past the end, we don't want to tack anything on
if (dist == rndf_dist_min)
{
rndfNextPoint = LinePath.PointOnPath.Invalid;
}
break;
}
// check the angle made by the last segment of center line and the segment
// formed between the end point of the center line and this new point
double angle = Math.Acos(centerLineEndSegmentVec.Dot((rndfNextPoint.Location - laneModelEndLoc).Normalize()));
// check if the angle satisfies the threshold or we're increasing the angle
if (Math.Abs(angle) < rndf_angle_threshold || (!double.IsNaN(lastAngle) && angle > lastAngle))
{
// break out of the loop, we're done searching
break;
}
lastAngle = angle;
}
}
// tack on the rndf starting at next point going to the end
if (rndfNextPoint.Valid)
{
LinePath subPath = rndfPath.SubPath(rndfNextPoint, rndfPath.EndPoint);
centerLine.AddRange(subPath);
// insert the lane model end point into the sub path
subPath.Insert(0, laneModelEndLoc);
// get the angles
List <Pair <int, double> > angles = subPath.GetIntersectionAngles(0, subPath.Count - 1);
// add the width of the path inflated by the angles
for (int i = 0; i < angles.Count; i++)
{
// calculate the width expansion factor
// 90 deg, 3x width
// 45 deg, 1.5x width
// 0 deg, 1x width
double widthFactor = Math.Pow(angles[i].Right / (Math.PI / 2.0), 2) * 2 + 1;
// add the width value
widthValue.Add(widthFactor * laneModel.Width / 2);
}
// add the final width
widthValue.Add(laneModel.Width / 2);
}
// set the rndf path start point to be the point we used
rndfPathStartPoint = rndfNextPoint;
}
// for now, calculate the left and right bounds the same way we do for the path lane model
// TODO: figure out if we want to do this more intelligently using knowledge of the lane model uncertainty
LinePath leftBound = centerLine.ShiftLateral(widthValue.ToArray());
// get the next shifts
for (int i = 0; i < widthValue.Count; i++)
{
widthValue[i] = -widthValue[i];
}
LinePath rightBound = centerLine.ShiftLateral(widthValue.ToArray());
// build the final lane model
finalLaneModel = new CombinedLaneModel(centerLine, leftBound, rightBound, laneModel.Width, rndfPathTimestamp);
}
SendLaneModelToUI(finalLaneModel, rndfPathTimestamp);
// output the fit result
return(finalLaneModel);
}
private ILaneModel GetLaneModel(LocalLaneModel laneModel, LinePath rndfPath, double rndfPathWidth, CarTimestamp rndfPathTimestamp)
{
// check the lane model probability
if (laneModel.Probability < lane_probability_reject_threshold) {
// we're rejecting this, just return a path lane model
return new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth);
}
// project the lane model's path into the rndf path's timestamp
RelativeTransform relTransform = Services.RelativePose.GetTransform(localRoadModel.Timestamp, rndfPathTimestamp);
LinePath laneModelPath = laneModel.LanePath.Transform(relTransform);
// iterate through the waypoints in the RNDF path and project onto the lane model
// the first one that is over the threshold, we consider the waypoint before as a potential ending point
LinePath.PointOnPath laneModelDeviationEndPoint = new LinePath.PointOnPath();
// flag indicating if any of the waypoint tests failed because of the devation was too high
bool anyDeviationTooHigh = false;
// number of waypoints accepted
int numWaypointsAccepted = 0;
// get the vehicle's position on the rndf path
LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint;
// get the vehicle's position on the lane model
LinePath.PointOnPath laneModelZeroPoint = laneModelPath.ZeroPoint;
// get the last point we want to consider on the lane model
LinePath.PointOnPath laneModelFarthestPoint = laneModelPath.AdvancePoint(laneModelZeroPoint, lane_model_max_dist);
// start walking forward through the waypoints on the rndf path
// this loop will implicitly exit when we're past the end of the lane model as the waypoints
// will stop being close to the lane model (GetClosestPoint returns the end point if we're past the
// end of the path)
for (int i = rndfZeroPoint.Index+1; i < rndfPath.Count; i++) {
// get the waypoint
Coordinates rndfWaypoint = rndfPath[i];
// get the closest point on the lane model
LinePath.PointOnPath laneModelClosestPoint = laneModelPath.GetClosestPoint(rndfWaypoint);
// compute the distance between the two
double deviation = rndfWaypoint.DistanceTo(laneModelClosestPoint.Location);
// if this is above the deviation threshold, leave the loop
if (deviation > lane_deviation_reject_threshold || laneModelClosestPoint > laneModelFarthestPoint) {
// if we're at the end of the lane model path, we don't want to consider this a rejection
if (laneModelClosestPoint < laneModelFarthestPoint) {
// mark that at least on deviation was too high
anyDeviationTooHigh = true;
}
break;
}
// increment the number of waypoint accepted
numWaypointsAccepted++;
// update the end point of where we're valid as the local road model was OK up to this point
laneModelDeviationEndPoint = laneModelClosestPoint;
}
// go through and figure out how far out the variance is within tolerance
LinePath.PointOnPath laneModelVarianceEndPoint = new LinePath.PointOnPath();
// walk forward from this point until the end of the lane mode path
for (int i = laneModelZeroPoint.Index+1; i < laneModelPath.Count; i++) {
// check if we're within the variance toleration
if (laneModel.LaneYVariance[i] <= y_var_reject_threshold) {
// we are, update the point on path
laneModelVarianceEndPoint = laneModelPath.GetPointOnPath(i);
}
else {
// we are out of tolerance, break out of the loop
break;
}
}
// now figure out everything out
// determine waypoint rejection status
WaypointRejectionResult waypointRejectionResult;
if (laneModelDeviationEndPoint.Valid) {
// if the point is valid, that we had at least one waypoint that was ok
// check if any waypoints were rejected
if (anyDeviationTooHigh) {
// some waypoint was ok, so we know that at least one waypoint was accepted
waypointRejectionResult = WaypointRejectionResult.SomeWaypointsAccepted;
}
else {
// no waypoint triggered a rejection, but at least one was good
waypointRejectionResult = WaypointRejectionResult.AllWaypointsAccepted;
}
}
else {
// the point is not valid, so we either had no waypoints or we had all rejections
if (anyDeviationTooHigh) {
// the first waypoint was rejected, so all are rejected
waypointRejectionResult = WaypointRejectionResult.AllWaypointsRejected;
}
else {
// the first waypoint (if any) was past the end of the lane model
waypointRejectionResult = WaypointRejectionResult.NoWaypoints;
}
}
// criteria for determining if this path is valid:
// - if some or all waypoints were accepted, than this is probably a good path
// - if some of the waypoints were accepted, we go no farther than the last waypoint that was accepted
// - if there were no waypoints, this is a potentially dangerous situation since we can't reject
// or confirm the local road model. for now, we'll assume that it is correct but this may need to change
// if we start handling intersections in this framework
// - if all waypoints were rejected, than we don't use the local road model
// - go no farther than the laneModelVarianceEndPoint, which is the last point where the y-variance of
// the lane model was in tolerance
// now build out the lane model
ILaneModel finalLaneModel;
// check if we rejected all waypoints or no lane model points satisified the variance threshold
if (waypointRejectionResult == WaypointRejectionResult.AllWaypointsRejected || !laneModelVarianceEndPoint.Valid) {
// want to just use the path lane model
finalLaneModel = new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth);
}
else {
// we'll use the lane model
// need to build up the center line as well as left and right bounds
// to build up the center line, use the lane model as far as we feel comfortable (limited by either variance
// or by rejections) and then use the rndf lane after that.
LinePath centerLine = new LinePath();
// figure out the max distance
// if there were no waypoints, set the laneModelDeviationEndPoint to the end of the lane model
if (waypointRejectionResult == WaypointRejectionResult.NoWaypoints) {
laneModelDeviationEndPoint = laneModelFarthestPoint;
}
// figure out the closer of the end points
LinePath.PointOnPath laneModelEndPoint = (laneModelDeviationEndPoint < laneModelVarianceEndPoint) ? laneModelDeviationEndPoint : laneModelVarianceEndPoint;
bool endAtWaypoint = laneModelEndPoint == laneModelDeviationEndPoint;
// add the lane model to the center line
centerLine.AddRange(laneModelPath.GetSubpathEnumerator(laneModelZeroPoint, laneModelEndPoint));
// create a list to hold the width expansion values
List<double> widthValue = new List<double>();
// make the width expansion values the width of the path plus the 1-sigma values
for (int i = laneModelZeroPoint.Index; i < laneModelZeroPoint.Index+centerLine.Count; i++) {
widthValue.Add(laneModel.Width/2.0 + Math.Sqrt(laneModel.LaneYVariance[i]));
}
// now figure out how to add the rndf path
// get the projection of the lane model end point on the rndf path
LinePath.PointOnPath rndfPathStartPoint = rndfPath.GetClosestPoint(laneModelEndPoint.Location);
// if the closest point is past the end of rndf path, then we don't want to tack anything on
if (rndfPathStartPoint != rndfPath.EndPoint) {
// get the last segment of the new center line
Coordinates centerLineEndSegmentVec = centerLine.EndSegment.UnitVector;
// get the last point of the new center line
Coordinates laneModelEndLoc = laneModelEndPoint.Location;
// now figure out the distance to the next waypoint
LinePath.PointOnPath rndfNextPoint = new LinePath.PointOnPath();
// figure out if we're ending at a waypoint or not
if (endAtWaypoint) {
rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index+1);
// if the distance from the start point to the next point is less than rndf_dist_min, then
// use the waypont after
double dist = rndfPath.DistanceBetween(rndfPathStartPoint, rndfNextPoint);
if (dist < rndf_dist_min) {
if (rndfPathStartPoint.Index < rndfPath.Count-2) {
rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index + 2);
}
else if (rndfPath.DistanceBetween(rndfPathStartPoint, rndfPath.EndPoint) < rndf_dist_min) {
rndfNextPoint = LinePath.PointOnPath.Invalid;
}
else {
rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, rndf_dist_min*2);
}
}
}
else {
// track the last angle we had
double lastAngle = double.NaN;
// walk down the rndf path until we find a valid point
for (double dist = rndf_dist_min; dist <= rndf_dist_max; dist += rndf_dist_step) {
// advance from the start point by dist
double distTemp = dist;
rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, ref distTemp);
// if the distTemp is > 0, then we're past the end of the path
if (distTemp > 0) {
// if we're immediately past the end, we don't want to tack anything on
if (dist == rndf_dist_min) {
rndfNextPoint = LinePath.PointOnPath.Invalid;
}
break;
}
// check the angle made by the last segment of center line and the segment
// formed between the end point of the center line and this new point
double angle = Math.Acos(centerLineEndSegmentVec.Dot((rndfNextPoint.Location-laneModelEndLoc).Normalize()));
// check if the angle satisfies the threshold or we're increasing the angle
if (Math.Abs(angle) < rndf_angle_threshold || (!double.IsNaN(lastAngle) && angle > lastAngle)) {
// break out of the loop, we're done searching
break;
}
lastAngle = angle;
}
}
// tack on the rndf starting at next point going to the end
if (rndfNextPoint.Valid) {
LinePath subPath = rndfPath.SubPath(rndfNextPoint, rndfPath.EndPoint);
centerLine.AddRange(subPath);
// insert the lane model end point into the sub path
subPath.Insert(0, laneModelEndLoc);
// get the angles
List<Pair<int, double>> angles = subPath.GetIntersectionAngles(0, subPath.Count-1);
// add the width of the path inflated by the angles
for (int i = 0; i < angles.Count; i++) {
// calculate the width expansion factor
// 90 deg, 3x width
// 45 deg, 1.5x width
// 0 deg, 1x width
double widthFactor = Math.Pow(angles[i].Right/(Math.PI/2.0), 2)*2 + 1;
// add the width value
widthValue.Add(widthFactor*laneModel.Width/2);
}
// add the final width
widthValue.Add(laneModel.Width/2);
}
// set the rndf path start point to be the point we used
rndfPathStartPoint = rndfNextPoint;
}
// for now, calculate the left and right bounds the same way we do for the path lane model
// TODO: figure out if we want to do this more intelligently using knowledge of the lane model uncertainty
LinePath leftBound = centerLine.ShiftLateral(widthValue.ToArray());
// get the next shifts
for (int i = 0; i < widthValue.Count; i++) { widthValue[i] = -widthValue[i]; }
LinePath rightBound = centerLine.ShiftLateral(widthValue.ToArray());
// build the final lane model
finalLaneModel = new CombinedLaneModel(centerLine, leftBound, rightBound, laneModel.Width, rndfPathTimestamp);
}
SendLaneModelToUI(finalLaneModel, rndfPathTimestamp);
// output the fit result
return finalLaneModel;
}
private PlanningPhase PlanStartingLane(LinePath curStartingLane, LinePath curEndingLane)
{
ILaneModel startingLaneModel = Services.RoadModelProvider.GetLaneModel(curStartingLane, startingLaneWidth + extraWidth, curTimestamp, startingLanesLeft, startingLanesRight);
// figure out where we are along the starting lane path
LinePath.PointOnPath startingLanePoint = curStartingLane.ZeroPoint;
// calculate the planning distance
double planningDistance = GetPlanningDistance();
if (sparse && planningDistance > 10) {
planningDistance = 10;
}
// get the distance to the end point of the lane from our current point
double remainingDistance = curStartingLane.DistanceBetween(startingLanePoint, curStartingLane.EndPoint);
double? boundStartDistMin;
double? boundEndDistMax;
DetermineSparseStarting(planningDistance, out boundStartDistMin, out boundEndDistMax);
double avoidanceExtra = sparse ? 5 : 7.5;
// linearize the lane model
LinePath centerLine, leftBound, rightBound;
if (boundEndDistMax != null || boundStartDistMin != null) {
if (boundEndDistMax != null) {
boundEndDistMax = Math.Min(boundEndDistMax.Value, remainingDistance - starting_lane_trim_dist);
}
else {
boundEndDistMax = remainingDistance - starting_lane_trim_dist;
}
LinearizeStayInLane(startingLaneModel, planningDistance + avoidanceExtra, null, boundEndDistMax, boundStartDistMin, null, curTimestamp, out centerLine, out leftBound, out rightBound);
}
else {
LinearizeStayInLane(startingLaneModel, planningDistance + avoidanceExtra, null, remainingDistance - starting_lane_trim_dist, null, curTimestamp,
out centerLine, out leftBound, out rightBound);
}
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);
// add to the planning setup
avoidanceBasePath = centerLine;
double targetDist = Math.Max(centerLine.PathLength-avoidanceExtra, planningDistance);
smootherBasePath = centerLine.SubPath(centerLine.StartPoint, targetDist);
// calculate time to collision of opposing obstacle if it exists
LinePath targetLine = centerLine;
double targetWeight = default_lane_alpha_w;
if (sparse) {
targetWeight = 0.00000025;
}
else if (oncomingVehicleExists) {
double shiftDist = -(TahoeParams.T + 1);
targetLine = centerLine.ShiftLateral(shiftDist);
targetWeight = 0.01;
}
else if (opposingLaneVehicleExists) {
double timeToCollision = opposingLaneVehicleDist/(Math.Abs(vs.speed)+Math.Abs(opposingLaneVehicleSpeed));
Services.Dataset.ItemAs<double>("time to collision").Add(timeToCollision, curTimestamp);
if (timeToCollision < 5) {
// shift to the right
double laneWidth = startingLaneModel.Width;
double shiftDist = -Math.Min(TahoeParams.T/2.0, laneWidth/2.0 - TahoeParams.T/2.0);
targetLine = centerLine.ShiftLateral(shiftDist);
}
}
// set up the planning
AddTargetPath(targetLine, targetWeight);
if (!sparse || boundStartDistMin != null || boundEndDistMax != null) {
// add the lane bounds
AddLeftBound(leftBound, true);
if (!oncomingVehicleExists) {
AddRightBound(rightBound, false);
}
}
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;
LinePath.PointOnPath startingLaneEndPoint = curStartingLane.AdvancePoint(startingLanePoint, ref advanceDist);
LinePath combinedPath = curStartingLane.SubPath(startingLanePoint, startingLaneEndPoint);
Coordinates endingLaneFirstPoint = curEndingLane[0];
Coordinates startingLaneLastPoint = curStartingLane.EndPoint.Location;
double intersectionDist = startingLaneLastPoint.DistanceTo(endingLaneFirstPoint);
// add a portion of the ending lane of the appropriate length
LinePath.PointOnPath endingLanePoint = curEndingLane.AdvancePoint(curEndingLane.StartPoint, Math.Max(advanceDist - intersectionDist, 5));
int endIndex = endingLanePoint.Index + 1;
combinedPath.AddRange(curEndingLane.GetSubpathEnumerator(0, endIndex));
// add to the planning setup
settings.maxSpeed = GetMaxSpeed(combinedPath, combinedPath.StartPoint);
settings.maxEndingSpeed = GetMaxSpeed(combinedPath, combinedPath.AdvancePoint(combinedPath.StartPoint, planningDistance));
if (sparse) {
// limit to 5 mph
laneWidthAtPathEnd = 20;
pathAngleCheckMax = 50;
pathAngleMax = 5 * Math.PI / 180.0;
settings.maxSpeed = Math.Min(settings.maxSpeed, 2.2352);
LinePath leftEdge = RoadEdge.GetLeftEdgeLine();
LinePath rightEdge = RoadEdge.GetRightEdgeLine();
if (leftEdge != null) {
leftLaneBounds.Add(new Boundary(leftEdge, 0.1, 1, 100, false));
}
if (rightEdge != null) {
rightLaneBounds.Add(new Boundary(rightEdge, 0.1, 1, 100, false));
}
maxAvoidanceBasePathAdvancePoint = avoidanceBasePath.AdvancePoint(avoidanceBasePath.EndPoint, -2);
//maxSmootherBasePathAdvancePoint = smootherBasePath.AdvancePoint(smootherBasePath.EndPoint, -2);
PlanningResult result = new PlanningResult();
ISteeringCommandGenerator commandGenerator = SparseArcVoting.SparcVote(ref prevCurvature, goalPoint);
if (commandGenerator == null) {
// we have a block, report dynamically infeasible
result = OnDynamicallyInfeasible(null, null);
}
else {
result.steeringCommandGenerator = commandGenerator;
result.commandLabel = commandLabel;
}
Track(result, commandLabel);
if (planningDistance > remainingDistance) {
return PlanningPhase.BeginEnteringTurn;
}
else {
return PlanningPhase.StartingLane;
}
}
else if (oncomingVehicleExists) {
laneWidthAtPathEnd = 10;
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "max speed set to {0}", settings.maxSpeed);
SmoothAndTrack(commandLabel, true);
if (cancelled) return PlanningPhase.StartingLane;
// check if we need to transition to the turn phase for the next iteration
// if the planning distance is greater than the remaining distance, i.e. we want to plan into the intersection, then
// we want to partially run the turn behavior next iteration
if (planningDistance > remainingDistance) {
return PlanningPhase.BeginEnteringTurn;
}
else {
return PlanningPhase.StartingLane;
}
}
private PlanningPhase PlanEnteringTurn(LinePath curStartingLane, LinePath curEndingLane)
{
// figure out where we are along the starting lane path
LinePath.PointOnPath startingLanePoint = curStartingLane.ZeroPoint;
// get the distance to the end point of the lane from our current point
double remainingDistance = curStartingLane.DistanceBetween(startingLanePoint, curStartingLane.EndPoint);
// immediately switch to the mid turn mode if the remaining distance is less than the front-length
// this prevents problems with the lane linearization extending out past the end of the lane
if (remainingDistance < TahoeParams.FL) {
return PlanMidTurn(curStartingLane, curEndingLane);
}
// get the lane mode for the starting lane
ILaneModel startingLaneModel = Services.RoadModelProvider.GetLaneModel(curStartingLane, startingLaneWidth + extraWidth, curTimestamp, startingLanesLeft, startingLanesRight);
double? boundStartDistMin;
double? boundEndDistMax;
DetermineSparseStarting(remainingDistance - starting_lane_trim_dist, out boundStartDistMin, out boundEndDistMax);
// linearize the lane model
LinePath centerLine, leftBound, rightBound;
if (boundEndDistMax != null || boundStartDistMin != null) {
if (boundEndDistMax != null) {
boundEndDistMax = Math.Min(boundEndDistMax.Value, remainingDistance - starting_lane_trim_dist);
}
else {
boundEndDistMax = remainingDistance - starting_lane_trim_dist;
}
LinearizeStayInLane(startingLaneModel, remainingDistance, 0, boundEndDistMax, boundStartDistMin, null, curTimestamp, out centerLine, out leftBound, out rightBound);
}
else {
LinearizeStayInLane(startingLaneModel, remainingDistance, 0, remainingDistance - starting_lane_trim_dist, null, curTimestamp,
out centerLine, out leftBound, out rightBound);
}
// create a copy so when we add to the center line later we don't change this version
LinePath startingCenterLine = centerLine.Clone();
// calculate time to collision of opposing obstacle if it exists
LinePath targetLine = startingCenterLine;
double targetWeight = default_lane_alpha_w;
if (oncomingVehicleExists) {
double shiftDist = -(TahoeParams.T + 1);
targetLine = centerLine.ShiftLateral(shiftDist);
targetWeight = 0.01;
}
if (opposingLaneVehicleExists) {
double timeToCollision = opposingLaneVehicleDist/(Math.Abs(vs.speed)+Math.Abs(opposingLaneVehicleSpeed));
Services.Dataset.ItemAs<double>("time to collision").Add(timeToCollision, curTimestamp);
if (timeToCollision < 5) {
// shift to the right
double laneWidth = startingLaneModel.Width;
double shiftDist = -Math.Min(TahoeParams.T/2.0, laneWidth/2.0 - TahoeParams.T/2.0);
targetLine = centerLine.ShiftLateral(shiftDist);
}
}
// set up the planning
AddTargetPath(targetLine, targetWeight);
// add the lane bounds
if (!sparse || boundStartDistMin != null || boundEndDistMax != null) {
// add the lane bounds
AddLeftBound(leftBound, true);
if (!oncomingVehicleExists) {
AddRightBound(rightBound, false);
}
}
LinePath endingCenterLine, endingLeftBound, endingRightBound;
LinearizeLane(curEndingLane, endingLaneWidth, 0, 5, 2, 6, out endingCenterLine, out endingLeftBound, out endingRightBound);
// add the ending lane center line to the target paths
AddTargetPath(endingCenterLine, 0.001);
// add the lane bounds
AddLeftBound(endingLeftBound, false);
AddRightBound(endingRightBound, false);
// intert a small portion of the ending lane
centerLine.AddRange(endingCenterLine);
// get the intersection pull path
LinePath intersectionPullPath = new LinePath();
double pullWeighting = 0;
GetIntersectionPullPath(startingCenterLine, 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;
LinePath.PointOnPath startingLaneEndPoint = curStartingLane.AdvancePoint(startingLanePoint, ref advanceDist);
LinePath combinedPath = curStartingLane.SubPath(startingLanePoint, startingLaneEndPoint);
if (advanceDist > 0) {
Coordinates endingLaneFirstPoint = curEndingLane[0];
Coordinates startingLaneLastPoint = curStartingLane.EndPoint.Location;
double intersectionDist = startingLaneLastPoint.DistanceTo(endingLaneFirstPoint);
double addDist = Math.Max(advanceDist - intersectionDist, 5);
// add a portion of the ending lane of the appropriate length
LinePath.PointOnPath endingLanePoint = curEndingLane.AdvancePoint(curEndingLane.StartPoint, addDist);
int endIndex = endingLanePoint.Index + 1;
combinedPath.AddRange(curEndingLane.GetSubpathEnumerator(0, endIndex));
}
settings.maxSpeed = GetMaxSpeed(combinedPath, combinedPath.StartPoint);
if (sparse) {
// limit to 5 mph
//laneWidthAtPathEnd = 20;
settings.maxSpeed = Math.Min(settings.maxSpeed, 2.2352);
LinePath leftEdge = RoadEdge.GetLeftEdgeLine();
LinePath rightEdge = RoadEdge.GetRightEdgeLine();
if (leftEdge != null) {
additionalLeftBounds.Add(new Boundary(leftEdge, 0.1, 1, 100, false));
}
if (rightEdge != null) {
additionalRightBounds.Add(new Boundary(rightEdge, 0.1, 1, 100, false));
}
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "max speed set to {0}", settings.maxSpeed);
// set the avoidance path to be the previously smoothed path
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp);
avoidanceBasePath = turnBasePath.Transform(absTransform);
avoidanceBasePath = avoidanceBasePath.SubPath(avoidanceBasePath.ZeroPoint, avoidanceBasePath.EndPoint);
smootherBasePath = avoidanceBasePath.Clone();
maxSmootherBasePathAdvancePoint = smootherBasePath.GetClosestPoint(startingCenterLine.EndPoint.Location);
// set auxillary settings
settings.endingHeading = centerLine.EndSegment.UnitVector.ArcTan;
//useAvoidancePath = true;
SmoothAndTrack(commandLabel, true);
if (cancelled) return PlanningPhase.EnteringTurn;
// 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 <= TahoeParams.FL) {
return PlanningPhase.MidTurn;
}
else {
return PlanningPhase.EnteringTurn;
}
}
private PlanningPhase PlanBeginningTurn(LinePath curStartingLane, LinePath curEndingLane)
{
// figure out where we are along the starting lane path
LinePath.PointOnPath startingLanePoint = curStartingLane.ZeroPoint;
// get the distance to the end point of the lane from our current point
double remainingDistance = curStartingLane.DistanceBetween(startingLanePoint, curStartingLane.EndPoint);
// get the lane mode for the starting lane
ILaneModel startingLaneModel = Services.RoadModelProvider.GetLaneModel(curStartingLane, startingLaneWidth + extraWidth, curTimestamp, startingLanesLeft, startingLanesRight);
double? boundStartDistMin;
double? boundEndDistMax;
DetermineSparseStarting(remainingDistance - starting_lane_trim_dist, out boundStartDistMin, out boundEndDistMax);
// linearize the lane model
LinePath centerLine, leftBound, rightBound;
if (boundEndDistMax != null || boundStartDistMin != null) {
if (boundEndDistMax != null) {
boundEndDistMax = Math.Min(boundEndDistMax.Value, remainingDistance - starting_lane_trim_dist);
}
else {
boundEndDistMax = remainingDistance - starting_lane_trim_dist;
}
LinearizeStayInLane(startingLaneModel, remainingDistance, 0, boundEndDistMax, boundStartDistMin, null, curTimestamp, out centerLine, out leftBound, out rightBound);
}
else {
LinearizeStayInLane(startingLaneModel, remainingDistance, 0, remainingDistance - starting_lane_trim_dist, null, curTimestamp,
out centerLine, out leftBound, out rightBound);
}
//LinearizeLane(curStartingLane, startingLaneWidth, TahoeParams.FL, remainingDistance, 5, remainingDistance - 5, out centerLine, out leftBound, out rightBound);
// create a copy so when we add to the center line later we don't change this version
LinePath startingCenterLine = centerLine.Clone();
// calculate time to collision of opposing obstacle if it exists
LinePath targetLine = startingCenterLine;
if (opposingLaneVehicleExists) {
double timeToCollision = opposingLaneVehicleDist/(Math.Abs(vs.speed)+Math.Abs(opposingLaneVehicleSpeed));
if (timeToCollision < 3) {
// shift to the right
double shiftDist = -TahoeParams.T/2.0;
targetLine = centerLine.ShiftLateral(shiftDist);
}
}
// set up the planning
AddTargetPath(targetLine, default_lane_alpha_w);
if (!sparse || boundStartDistMin != null || boundEndDistMax != null) {
// add the lane bounds
AddLeftBound(leftBound, true);
if (!oncomingVehicleExists) {
AddRightBound(rightBound, false);
}
}
LinePath endingCenterLine, endingLeftBound, endingRightBound;
LinearizeLane(curEndingLane, endingLaneWidth, 0, 5, 2, 6, out endingCenterLine, out endingLeftBound, out endingRightBound);
// add the ending lane center line to the target paths
AddTargetPath(endingCenterLine, default_lane_alpha_w);
// add the lane bounds
AddLeftBound(endingLeftBound, false);
AddRightBound(endingRightBound, false);
// intert a small portion of the ending lane
centerLine.AddRange(endingCenterLine);
// add to the planning setup
smootherBasePath = centerLine;
maxSmootherBasePathAdvancePoint = smootherBasePath.GetClosestPoint(startingCenterLine.EndPoint.Location);
// get the intersection pull path
LinePath intersectionPullPath = new LinePath();
double pullWeighting = 0;
GetIntersectionPullPath(startingCenterLine, 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);
// calculate max speed for the path over the next 120 meters
double advanceDist = speed_path_advance_dist;
LinePath.PointOnPath startingLaneEndPoint = curStartingLane.AdvancePoint(startingLanePoint, ref advanceDist);
LinePath combinedPath = curStartingLane.SubPath(startingLanePoint, startingLaneEndPoint);
if (advanceDist > 0) {
Coordinates endingLaneFirstPoint = curEndingLane[0];
Coordinates startingLaneLastPoint = curStartingLane.EndPoint.Location;
double intersectionDist = startingLaneLastPoint.DistanceTo(endingLaneFirstPoint);
double addDist = Math.Max(advanceDist - intersectionDist, 5);
// add a portion of the ending lane of the appropriate length
LinePath.PointOnPath endingLanePoint = curEndingLane.AdvancePoint(curEndingLane.StartPoint, addDist);
int endIndex = endingLanePoint.Index + 1;
combinedPath.AddRange(curEndingLane.GetSubpathEnumerator(0, endIndex));
}
// set the planning settings max speed
settings.maxSpeed = GetMaxSpeed(combinedPath, combinedPath.StartPoint);
if (sparse) {
// limit to 5 mph
//laneWidthAtPathEnd = 20;
settings.maxSpeed = Math.Min(settings.maxSpeed, 2.2352);
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "max speed set to {0}", settings.maxSpeed);
// set auxillary settings
settings.endingHeading = centerLine.EndSegment.UnitVector.ArcTan;
PlanningResult planningResult = Smooth(false);
if (!planningResult.pathBlocked && !planningResult.dynamicallyInfeasible) {
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp);
turnBasePath = planningResult.smoothedPath.Transform(absTransform.Invert());
return PlanningPhase.EnteringTurn;
}
else {
// keep trying I guess
return PlanningPhase.BeginEnteringTurn;
}
}
public override void Process(object param)
{
if (!base.BeginProcess())
{
return;
}
// check if we're given a param
if (param != null && param is TurnBehavior)
{
TurnBehavior turnParam = (TurnBehavior)param;
HandleTurnBehavior(turnParam);
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "turn behavior - got new param, speed command {0}", turnParam.SpeedCommand);
}
LinePath curTargetPath = targetPath.Clone();
LinePath curLeftBound = null;
if (leftBound != null)
{
curLeftBound = leftBound.Clone();
}
LinePath curRightBound = null;
if (rightBound != null)
{
curRightBound = rightBound.Clone();
}
// transform the path into the current timestamp
if (behaviorTimestamp != curTimestamp)
{
// get the relative transform
RelativeTransform relTransform = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "in turn behavior, transforming from {0}->{1}, wanted {2}->{3}", relTransform.OriginTimestamp, relTransform.EndTimestamp, behaviorTimestamp, curTimestamp);
curTargetPath.TransformInPlace(relTransform);
if (curLeftBound != null)
{
curLeftBound.TransformInPlace(relTransform);
}
if (curRightBound != null)
{
curRightBound.TransformInPlace(relTransform);
}
}
// get the distance between our current location and the start point
double distToStart = Coordinates.Zero.DistanceTo(curTargetPath[0]);
double startDist = Math.Max(0, TahoeParams.FL - distToStart);
// extract off the first 5 m of the target path
LinePath.PointOnPath endTarget = curTargetPath.AdvancePoint(curTargetPath.StartPoint, TahoeParams.VL);
curTargetPath = curTargetPath.SubPath(curTargetPath.StartPoint, endTarget);
curTargetPath = curTargetPath.RemoveZeroLengthSegments();
// adjust the left bound and right bounds starting distance
if (curLeftBound != null)
{
LinePath.PointOnPath leftBoundStart = curLeftBound.AdvancePoint(curLeftBound.StartPoint, 2);
curLeftBound = curLeftBound.SubPath(leftBoundStart, curLeftBound.EndPoint);
AddLeftBound(curLeftBound, false);
Services.UIService.PushLineList(curLeftBound, curTimestamp, "left bound", true);
}
if (curRightBound != null)
{
LinePath.PointOnPath rightBoundStart = curRightBound.AdvancePoint(curRightBound.StartPoint, 2);
curRightBound = curRightBound.SubPath(rightBoundStart, curRightBound.EndPoint);
AddRightBound(curRightBound, false);
Services.UIService.PushLineList(curRightBound, curTimestamp, "right bound", true);
}
if (cancelled)
{
return;
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "in turn behavior, dist to start: {0}", distToStart);
if (distToStart < TahoeParams.FL * 0.75)
{
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "in turn behavior, past start point of target path");
// return a completion report
ForwardCompletionReport(new SuccessCompletionReport(typeof(TurnBehavior)));
}
AddTargetPath(curTargetPath.Clone(), 0.005);
// transform the pseudo start lane to the current timestamp
AbsoluteTransformer trans = Services.StateProvider.GetAbsoluteTransformer(curTimestamp);
LinePath curStartPath = pseudoStartLane.Transform(trans);
// add the intersection pull path
LinePath intersectionPullPath = new LinePath();
double pullWeight = 0;
GetIntersectionPullPath(curStartPath, curTargetPath, intersectionPolygon, true, true, intersectionPullPath, ref pullWeight);
if (intersectionPullPath.Count > 0)
{
AddTargetPath(intersectionPullPath, pullWeight);
}
// set up planning details
// add our position to the current target path
LinePath origTargetPath = curTargetPath.Clone();
curTargetPath.Insert(0, new Coordinates(0, 0));
//curTargetPath.Insert(1, new Coordinates(1, 0));
smootherBasePath = curTargetPath;
// add the bounds
// calculate max speed
settings.maxSpeed = GetMaxSpeed(null, LinePath.PointOnPath.Invalid);
settings.Options.w_diff = 4;
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "max speed set to {0}", settings.maxSpeed);
if (cancelled)
{
return;
}
Services.UIService.PushLineList(curTargetPath, curTimestamp, "subpath", true);
// set the avoidance path to the previously planned path
// transform to the current timestamp
disablePathAngleCheck = true;
if (turnBasePath != null)
{
avoidanceBasePath = turnBasePath.Transform(trans);
avoidanceBasePath = avoidanceBasePath.SubPath(avoidanceBasePath.GetClosestPoint(Coordinates.Zero), avoidanceBasePath.EndPoint);
maxAvoidanceBasePathAdvancePoint = avoidanceBasePath.GetPointOnPath(1);
if (avoidanceBasePath.PathLength > 7.5)
{
smootherBasePath = avoidanceBasePath.SubPath(avoidanceBasePath.StartPoint, avoidanceBasePath.AdvancePoint(avoidanceBasePath.EndPoint, -7.5));
}
else
{
smootherBasePath = avoidanceBasePath.Clone();
}
disablePathAngleCheck = false;
}
// fill in auxiliary settings
if (curLeftBound != null || curRightBound != null)
{
settings.endingHeading = curTargetPath.EndSegment.UnitVector.ArcTan;
}
settings.endingPositionFixed = false;
settings.endingPositionMax = 16;
settings.endingPositionMin = -16;
if (curLeftBound != null && curLeftBound.Count > 0 && curTargetPath.Count > 0)
{
double leftWidth = curLeftBound[0].DistanceTo(origTargetPath[0]) - TahoeParams.T / 2;
settings.endingPositionMax = leftWidth;
settings.endingPositionFixed = true;
}
if (curRightBound != null && curRightBound.Count > 0 && curTargetPath.Count > 0)
{
double rightWidth = curRightBound[0].DistanceTo(origTargetPath[0]) - TahoeParams.T / 2;
settings.endingPositionFixed = true;
settings.endingPositionMin = -rightWidth;
}
//disablePathAngleCheck = true;
//useAvoidancePath = true;
// do the planning
SmoothAndTrack(commandLabel, true);
}
private void DoInitialPlan()
{
InitializePlanningSettings();
curTimestamp = Services.RelativePose.CurrentTimestamp;
vs = Services.StateProvider.GetVehicleState();
LinePath curTargetPath = targetPath.Clone();
LinePath curLeftBound = null;
if (leftBound != null)
{
curLeftBound = leftBound.Clone();
}
LinePath curRightBound = null;
if (rightBound != null)
{
curRightBound = rightBound.Clone();
}
// get the distance between our current location and the start point
double distToStart = Coordinates.Zero.DistanceTo(curTargetPath[0]);
// extract off the first 5 m of the target path
LinePath.PointOnPath endTarget = curTargetPath.AdvancePoint(curTargetPath.StartPoint, 12);
curTargetPath = curTargetPath.SubPath(curTargetPath.StartPoint, endTarget);
AddTargetPath(curTargetPath.Clone(), 0.005);
// adjust the left bound and right bounds starting distance
if (curLeftBound != null)
{
LinePath.PointOnPath leftBoundStart = curLeftBound.AdvancePoint(curLeftBound.StartPoint, 2);
curLeftBound = curLeftBound.SubPath(leftBoundStart, curLeftBound.EndPoint);
AddLeftBound(curLeftBound, false);
}
if (curRightBound != null)
{
LinePath.PointOnPath rightBoundStart = curRightBound.AdvancePoint(curRightBound.StartPoint, 2);
curRightBound = curRightBound.SubPath(rightBoundStart, curRightBound.EndPoint);
AddRightBound(curRightBound, false);
}
// add the intersection pull path
LinePath intersectionPullPath = new LinePath();
double pullWeight = 0;
AbsoluteTransformer trans = Services.StateProvider.GetAbsoluteTransformer(curTimestamp);
GetIntersectionPullPath(pseudoStartLane.Transform(trans), curTargetPath, intersectionPolygon, true, true, intersectionPullPath, ref pullWeight);
if (intersectionPullPath.Count > 0)
{
AddTargetPath(intersectionPullPath, pullWeight);
}
// set up planning details
// add our position to the current target path
curTargetPath.Insert(0, new Coordinates(0, 0));
smootherBasePath = curTargetPath;
// add the bounds
// calculate max speed
settings.maxSpeed = GetMaxSpeed(null, LinePath.PointOnPath.Invalid);
// fill in auxiliary settings
if (curLeftBound != null || curRightBound != null)
{
settings.endingHeading = curTargetPath.EndSegment.UnitVector.ArcTan;
}
disablePathAngleCheck = true;
settings.Options.w_diff = 4;
// do the planning
PlanningResult planningResult = Smooth(false);
// if the planning was a success, store the result
if (!planningResult.dynamicallyInfeasible)
{
// transform to absolute coordinates
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp);
turnBasePath = planningResult.smoothedPath.Transform(absTransform.Invert());
}
}
/// <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);
}
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 PlanningPhase PlanEndingLane(LinePath curEndingLane)
{
// get the desired planning distance
double planningDistance = GetPlanningDistance();
double remainingDistance = Coordinates.Zero.DistanceTo(curEndingLane.ZeroPoint.Location);
remainingDistance = Math.Max(0, remainingDistance);
double laneStartDist = Math.Max(TahoeParams.FL - remainingDistance, 0);
double boundStartDist = Math.Max(6 - remainingDistance, 2);
// get the road model
ILaneModel endingLaneModel = Services.RoadModelProvider.GetLaneModel(curEndingLane, endingLaneWidth, curTimestamp, endingLanesLeft, endingLanesRight);
// linearize the lane model
LinePath centerLine, leftBound, rightBound;
//LinearizeLane(curEndingLane, endingLaneWidth, laneStartDist, planningDistance, boundStartDist, TahoeParams.FL + planningDistance, out centerLine, out leftBound, out rightBound);
LinearizeStayInLane(endingLaneModel, planningDistance, laneStartDist, null, boundStartDist, curTimestamp, out centerLine, out leftBound, out rightBound);
// add to the planning setup
LinePath endingCenterLine = centerLine.Clone();
AddTargetPath(endingCenterLine, default_lane_alpha_w);
AddLeftBound(leftBound, true);
AddRightBound(rightBound, false);
// insert our position if we didn't include it already
if (laneStartDist == 0) {
centerLine.Insert(0, Coordinates.Zero);
}
smootherBasePath = centerLine;
if (remainingDistance > 0) {
maxSmootherBasePathAdvancePoint = centerLine.GetClosestPoint(endingCenterLine.StartPoint.Location);
}
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 60 meters
LinePath combinedPath = curEndingLane.SubPath(curEndingLane.StartPoint, 120);
//LaneSpeedHelper.CalculateMaxPathSpeed(combinedPath, combinedPath.StartPoint, combinedPath.EndPoint, ref maxSpeed);
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);
SmoothAndTrack(commandLabel, true);
return PlanningPhase.EndingLane;
}
protected virtual LinePath GetPathBlockedSubPath(LinePath basePath, double stopDist)
{
return basePath.SubPath(basePath.StartPoint, stopDist + TahoeParams.FL);
}
