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