コード例 #1
0
        public SmoothingResult PlanPath(LineList basePath, LineList targetPath,
                                        List <Boundary> leftBounds, List <Boundary> rightBounds, double initHeading,
                                        double maxSpeed, double startSpeed, double?endingHeading, CarTimestamp time, bool endingOffsetFixed)
        {
            PlanningSettings settings = new PlanningSettings();

            settings.basePath    = basePath;
            settings.targetPaths = new List <Boundary>();
            settings.targetPaths.Add(new Boundary(targetPath, 0, 0, settings.Options.alpha_w));
            settings.leftBounds          = leftBounds;
            settings.rightBounds         = rightBounds;
            settings.initHeading         = initHeading;
            settings.maxSpeed            = maxSpeed;
            settings.startSpeed          = startSpeed;
            settings.endingHeading       = endingHeading;
            settings.timestamp           = time;
            settings.endingPositionFixed = endingOffsetFixed;

            return(PlanPath(settings));
        }
コード例 #2
0
        public SmoothingResult PlanPath(LineList basePath, LineList targetPath, 
            List<Boundary> leftBounds, List<Boundary> rightBounds, double initHeading,
            double maxSpeed, double startSpeed, double? endingHeading, CarTimestamp time, bool endingOffsetFixed)
        {
            PlanningSettings settings = new PlanningSettings();
            settings.basePath = basePath;
            settings.targetPaths = new List<Boundary>();
            settings.targetPaths.Add(new Boundary(targetPath, 0, 0, settings.Options.alpha_w));
            settings.leftBounds = leftBounds;
            settings.rightBounds = rightBounds;
            settings.initHeading = initHeading;
            settings.maxSpeed = maxSpeed;
            settings.startSpeed = startSpeed;
            settings.endingHeading = endingHeading;
            settings.timestamp = time;
            settings.endingPositionFixed = endingOffsetFixed;

            return PlanPath(settings);
        }
コード例 #3
0
        public SmoothingResult PlanPath(PlanningSettings settings)
        {
            SmootherOptions opts = settings.Options;
            // for now, just run the smoothing
            opts.init_heading = settings.initHeading;
            opts.set_init_heading = true;

            opts.min_init_velocity = settings.startSpeed*0.5;
            opts.set_min_init_velocity = true;

            opts.max_init_velocity = Math.Max(settings.maxSpeed, settings.startSpeed);
            opts.set_max_init_velocity = true;

            opts.min_velocity = 0.1;
            opts.max_velocity = Math.Max(opts.min_velocity+0.1, settings.maxSpeed);

            opts.k_max = Math.Min(TahoeParams.CalculateCurvature(-TahoeParams.SW_max, settings.startSpeed), TahoeParams.CalculateCurvature(TahoeParams.SW_max, settings.startSpeed)) * 0.97;

            opts.generate_details = true;// GenerateDetails;

            if (settings.endingHeading != null) {
                opts.set_final_heading = true;
                opts.final_heading = settings.endingHeading.Value;
            }
            else {
                opts.set_final_heading = false;
            }

            opts.set_final_offset = settings.endingPositionFixed;
            opts.final_offset_min = settings.endingPositionMin;
            opts.final_offset_max = settings.endingPositionMax;

            if (settings.maxEndingSpeed != null) {
                opts.set_final_velocity_max = true;
                opts.final_velocity_max = Math.Max(opts.min_velocity+0.1, settings.maxEndingSpeed.Value);
            }
            else {
                opts.set_final_velocity_max = false;
            }

            opts.a_lat_max = 6;

            // create the boundary list
            List<UrbanChallenge.PathSmoothing.PathPoint> ret = new List<UrbanChallenge.PathSmoothing.PathPoint>();
            smoother = new PathSmoother();
            OperationalTrace.WriteVerbose("calling smooth path");
            SmoothResult result = smoother.SmoothPath(settings.basePath, settings.targetPaths, settings.leftBounds, settings.rightBounds, opts, ret);

            if (result != SmoothResult.Sucess) {
                OperationalTrace.WriteWarning("smooth path result: {0}", result);
            }
            else {
                OperationalTrace.WriteVerbose("smooth path result: {0}", result);
            }

            AvoidanceDetails details = null;
            if (opts.generate_details) {
                details = new AvoidanceDetails();
                details.leftBounds = settings.leftBounds;
                details.rightBounds = settings.rightBounds;
                details.smoothingDetails = smoother.GetSmoothingDetails();
                LastAvoidanceDetails = details;

                // push out the points
                Coordinates[] leftPoints = new Coordinates[details.smoothingDetails.leftBounds.Length];
                for (int i = 0; i < leftPoints.Length; i++) {
                    leftPoints[i] = details.smoothingDetails.leftBounds[i].point;
                }

                Coordinates[] rightPoints = new Coordinates[details.smoothingDetails.rightBounds.Length];
                for (int i = 0; i < rightPoints.Length; i++) {
                    rightPoints[i] = details.smoothingDetails.rightBounds[i].point;
                }

                Services.UIService.PushPoints(leftPoints, settings.timestamp, "left bound points", true);
                Services.UIService.PushPoints(rightPoints, settings.timestamp, "right bound points", true);
            }

            //if (result == SmoothResult.Sucess) {
                Coordinates[] points = new Coordinates[ret.Count];
                double[] speeds = new double[ret.Count];
                for (int i = 0; i < ret.Count; i++) {
                    points[i] = new Coordinates(ret[i].x, ret[i].y);
                    speeds[i] = ret[i].v;
                }

                SmoothedPath path = new SmoothedPath(settings.timestamp, points, speeds);

                return new SmoothingResult(result, path, details);
            /*}
            else {
                SmoothedPath path = new SmoothedPath(settings.timestamp, settings.basePath, null);

                return new SmoothingResult(result, path);
            }*/
        }
コード例 #4
0
        public SmoothingResult PlanPath(PlanningSettings settings)
        {
            SmootherOptions opts = settings.Options;

            // for now, just run the smoothing
            opts.init_heading     = settings.initHeading;
            opts.set_init_heading = true;

            opts.min_init_velocity     = settings.startSpeed * 0.5;
            opts.set_min_init_velocity = true;

            opts.max_init_velocity     = Math.Max(settings.maxSpeed, settings.startSpeed);
            opts.set_max_init_velocity = true;

            opts.min_velocity = 0.1;
            opts.max_velocity = Math.Max(opts.min_velocity + 0.1, settings.maxSpeed);

            opts.k_max = Math.Min(TahoeParams.CalculateCurvature(-TahoeParams.SW_max, settings.startSpeed), TahoeParams.CalculateCurvature(TahoeParams.SW_max, settings.startSpeed)) * 0.97;

            opts.generate_details = true;            // GenerateDetails;

            if (settings.endingHeading != null)
            {
                opts.set_final_heading = true;
                opts.final_heading     = settings.endingHeading.Value;
            }
            else
            {
                opts.set_final_heading = false;
            }

            opts.set_final_offset = settings.endingPositionFixed;
            opts.final_offset_min = settings.endingPositionMin;
            opts.final_offset_max = settings.endingPositionMax;


            if (settings.maxEndingSpeed != null)
            {
                opts.set_final_velocity_max = true;
                opts.final_velocity_max     = Math.Max(opts.min_velocity + 0.1, settings.maxEndingSpeed.Value);
            }
            else
            {
                opts.set_final_velocity_max = false;
            }

            opts.a_lat_max = 6;

            // create the boundary list
            List <UrbanChallenge.PathSmoothing.PathPoint> ret = new List <UrbanChallenge.PathSmoothing.PathPoint>();

            smoother = new PathSmoother();
            OperationalTrace.WriteVerbose("calling smooth path");
            SmoothResult result = smoother.SmoothPath(settings.basePath, settings.targetPaths, settings.leftBounds, settings.rightBounds, opts, ret);

            if (result != SmoothResult.Sucess)
            {
                OperationalTrace.WriteWarning("smooth path result: {0}", result);
            }
            else
            {
                OperationalTrace.WriteVerbose("smooth path result: {0}", result);
            }

            AvoidanceDetails details = null;

            if (opts.generate_details)
            {
                details                  = new AvoidanceDetails();
                details.leftBounds       = settings.leftBounds;
                details.rightBounds      = settings.rightBounds;
                details.smoothingDetails = smoother.GetSmoothingDetails();
                LastAvoidanceDetails     = details;

                // push out the points
                Coordinates[] leftPoints = new Coordinates[details.smoothingDetails.leftBounds.Length];
                for (int i = 0; i < leftPoints.Length; i++)
                {
                    leftPoints[i] = details.smoothingDetails.leftBounds[i].point;
                }

                Coordinates[] rightPoints = new Coordinates[details.smoothingDetails.rightBounds.Length];
                for (int i = 0; i < rightPoints.Length; i++)
                {
                    rightPoints[i] = details.smoothingDetails.rightBounds[i].point;
                }

                Services.UIService.PushPoints(leftPoints, settings.timestamp, "left bound points", true);
                Services.UIService.PushPoints(rightPoints, settings.timestamp, "right bound points", true);
            }

            //if (result == SmoothResult.Sucess) {
            Coordinates[] points = new Coordinates[ret.Count];
            double[]      speeds = new double[ret.Count];
            for (int i = 0; i < ret.Count; i++)
            {
                points[i] = new Coordinates(ret[i].x, ret[i].y);
                speeds[i] = ret[i].v;
            }

            SmoothedPath path = new SmoothedPath(settings.timestamp, points, speeds);

            return(new SmoothingResult(result, path, details));

            /*}
             * else {
             *      SmoothedPath path = new SmoothedPath(settings.timestamp, settings.basePath, null);
             *
             *      return new SmoothingResult(result, path);
             * }*/
        }
コード例 #5
0
        protected void InitializePlanningSettings()
        {
            settings = new PlanningSettings();
            smootherBasePath = null;
            maxSmootherBasePathAdvancePoint = LinePath.PointOnPath.Invalid;
            avoidanceBasePath = null;
            maxAvoidanceBasePathAdvancePoint = LinePath.PointOnPath.Invalid;
            smootherTargetPaths = new List<Boundary>();
            leftLaneBounds = new List<Boundary>();
            rightLaneBounds = new List<Boundary>();
            additionalLeftBounds = new List<Boundary>();
            additionalRightBounds = new List<Boundary>();
            extraObstacles = new List<Obstacle>();
            disablePathAngleCheck = false;
            pathAngleCheckMax = 20;
            pathAngleMax = 15 * Math.PI / 180.0;
            useAvoidancePath = false;
            laneWidthAtPathEnd = 3.5;

            settings.Options.alpha_w = 0.00025;
            settings.Options.alpha_d = 10;
            settings.Options.alpha_c = 10;
        }