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;
        }
Exemple #2
0
        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 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;
            }
        }