private void SmoothIt() { Stopwatch s = Stopwatch.StartNew(); SmootherOptions opt = PathSmoother.GetDefaultOptions(); if (optIPOPT.Checked) { opt.alg = SmoothingAlgorithm.Ipopt; } else { opt.alg = SmoothingAlgorithm.Loqo; } opt.alpha_w = getW(); opt.alpha_d = getD1(); opt.set_init_velocity = true; opt.init_velocity = getV(); opt.set_init_heading = true; opt.init_heading = Math.Atan2(pathPoints[1].Y - pathPoints[0].Y, pathPoints[1].X - pathPoints[0].X); opt.set_final_heading = true; opt.final_heading = Math.Atan2(pathPoints[pathPoints.Count - 1].Y - pathPoints[pathPoints.Count - 2].Y, pathPoints[pathPoints.Count - 1].X - pathPoints[pathPoints.Count - 2].X); opt.set_final_offset = true; List <PathPoint> path = new List <PathPoint>(); Boundary ub = new Boundary(); ub.Coords = ubPoints; ub.DesiredSpacing = 1; ub.MinSpacing = 0.25; ub.Polygon = false; List <Boundary> ub_bounds = new List <Boundary>(); ub_bounds.Add(ub); Boundary lb = new Boundary(); lb.Coords = lbPoints; lb.DesiredSpacing = 0.5; lb.MinSpacing = 0; lb.Polygon = false; List <Boundary> lb_bounds = new List <Boundary>(); lb_bounds.Add(lb); LineList basePath = new LineList(); basePath.AddRange(pathPoints); SmoothResult sr = SmoothResult.Error; try { sr = PathSmoother.SmoothPath(basePath, ub_bounds, lb_bounds, opt, path); } catch (Exception ex) { MessageBox.Show("exception: " + ex.Message); return; } smoothedPoints = path.ConvertAll <Coordinates>(delegate(PathPoint p) { return(new Coordinates(p.x, p.y)); }); s.Stop(); long ms = s.ElapsedMilliseconds; if (sr != SmoothResult.Sucess) { MessageBox.Show("Path smooth result: " + sr.ToString()); } MessageBox.Show("Elapsed MS: " + ms); picEntry.Invalidate(); }
public SmoothingResult(SmoothResult result, SmoothedPath path, AvoidanceDetails details) { this.result = result; this.path = path; this.details = details; }
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); * }*/ }