예제 #1
0
        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);
        }
        protected void LinearizeStayInLane(ILaneModel laneModel, double laneDist,
            double? laneStartDistMax, double? boundDistMax, double? boundStartDistMin, double? boundStartDistMax, CarTimestamp curTimestamp,
            out LinePath centerLine, out LinePath leftBound, out LinePath rightBound)
        {
            // get the center line, left bound and right bound
            LinearizationOptions laneOpts = new LinearizationOptions(0, laneDist, curTimestamp);
            centerLine = laneModel.LinearizeCenterLine(laneOpts);
            if (centerLine == null || centerLine.Count < 2) {
                BehaviorManager.TraceSource.TraceEvent(TraceEventType.Error, 0, "center line linearization SUCKS");
            }

            double leftStartDist = vs.speed*TahoeParams.actuation_delay + 1;
            double rightStartDist = vs.speed*TahoeParams.actuation_delay + 1;

            double offtrackDistanceBack = centerLine.ZeroPoint.OfftrackDistance(Coordinates.Zero);
            bool offtrackLeftBack = offtrackDistanceBack > 0;
            double additionalDistBack = 5*Math.Abs(offtrackDistanceBack)/laneModel.Width;
            if (offtrackLeftBack) {
                leftStartDist += additionalDistBack;
            }
            else {
                rightStartDist += additionalDistBack;
            }

            // now determine how to generate the left/right bounds
            // figure out the offtrack distance from the vehicle's front bumper
            double offtrackDistanceFront = centerLine.ZeroPoint.OfftrackDistance(new Coordinates(TahoeParams.FL, 0));

            // offtrackDistance > 0 => we're to left of path
            // offtrackDistance < 0 => we're to right of path
            bool offsetLeftFront = offtrackDistanceFront > 0;

            double additionalDistFront = 10*Math.Abs(offtrackDistanceFront)/laneModel.Width;
            if (offsetLeftFront) {
                leftStartDist += additionalDistFront;
            }
            else {
                rightStartDist += additionalDistFront;
            }

            if (boundStartDistMax.HasValue) {
                if (leftStartDist > boundStartDistMax.Value) {
                    leftStartDist = boundStartDistMax.Value;
                }

                if (rightStartDist > boundStartDistMax.Value) {
                    rightStartDist = boundStartDistMax.Value;
                }
            }

            if (boundStartDistMin.HasValue) {
                if (leftStartDist < boundStartDistMin.Value) {
                    leftStartDist = boundStartDistMin.Value;
                }

                if (rightStartDist < boundStartDistMin.Value) {
                    rightStartDist = boundStartDistMin.Value;
                }
            }

            double boundEndDist = laneDist + 5;
            if (boundDistMax.HasValue && boundDistMax.Value < boundEndDist) {
                boundEndDist = boundDistMax.Value;
            }

            laneOpts = new LinearizationOptions(leftStartDist, boundEndDist, curTimestamp);
            leftBound = laneModel.LinearizeLeftBound(laneOpts);
            laneOpts = new LinearizationOptions(rightStartDist, boundEndDist, curTimestamp);
            rightBound = laneModel.LinearizeRightBound(laneOpts);

            double laneStartDist = TahoeParams.FL;
            if (laneStartDistMax.HasValue && laneStartDistMax.Value < laneStartDist) {
                laneStartDist = laneStartDistMax.Value;
            }

            // remove the first FL distance of the center line
            if (laneStartDist > 0) {
                centerLine = centerLine.RemoveBefore(centerLine.AdvancePoint(centerLine.ZeroPoint, laneStartDist));
                centerLine.Insert(0, Coordinates.Zero);
            }
        }