public SmoothingResult PlanPath(LineList basePath, LineList targetPath, IList<LineList> leftBounds, IList<LineList> rightBounds, double initHeading, double maxSpeed, double startSpeed, double? endingHeading, CarTimestamp time, bool endingOffsetFixed) { // create the boundary list List<Boundary> upperBound = new List<Boundary>(); foreach (LineList leftBound in leftBounds) { Boundary ub0 = new Boundary(); ub0.Coords = leftBound; ub0.DesiredSpacing = 0.5; ub0.MinSpacing = 0.1; ub0.Polygon = false; upperBound.Add(ub0); } List<Boundary> lowerBound = new List<Boundary>(); foreach (LineList rightBound in rightBounds) { Boundary lb0 = new Boundary(); lb0.Coords = rightBound; lb0.DesiredSpacing = 0.5; lb0.MinSpacing = 0.1; lb0.Polygon = false; lowerBound.Add(lb0); } return PlanPath(basePath, targetPath, upperBound, lowerBound, initHeading, maxSpeed, startSpeed, endingHeading, time, endingOffsetFixed); }
public void Process(object param) { if (cancelled) return; DateTime start = HighResDateTime.Now; OperationalVehicleState vs = Services.StateProvider.GetVehicleState(); LinePath curBasePath = basePath; LinePath curLeftBound = leftBound; LinePath curRightBound = rightBound; // transform the base path to the current iteration CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp; if (pathTime != curTimestamp) { RelativeTransform relTransform = Services.RelativePose.GetTransform(pathTime, curTimestamp); curBasePath = curBasePath.Transform(relTransform); curLeftBound = curLeftBound.Transform(relTransform); curRightBound = curRightBound.Transform(relTransform); } // get the distance between the zero point and the start point double distToStart = Coordinates.Zero.DistanceTo(curBasePath.GetPoint(curBasePath.StartPoint)); // get the sub-path between 5 and 25 meters ahead double startDist = TahoeParams.FL; LinePath.PointOnPath startPoint = curBasePath.AdvancePoint(curBasePath.ZeroPoint, ref startDist); double endDist = 30; LinePath.PointOnPath endPoint = curBasePath.AdvancePoint(startPoint, ref endDist); if (startDist > 0) { // we've reached the end Services.BehaviorManager.Execute(new HoldBrakeBehavior(), null, false); return; } // get the sub-path LinePath subPath = curBasePath.SubPath(startPoint, endPoint); // add (0,0) as the starting point subPath.Insert(0, new Coordinates(0, 0)); // do the same for the left, right bound startDist = TahoeParams.FL; endDist = 40; startPoint = curLeftBound.AdvancePoint(curLeftBound.ZeroPoint, startDist); endPoint = curLeftBound.AdvancePoint(startPoint, endDist); curLeftBound = curLeftBound.SubPath(startPoint, endPoint); startPoint = curRightBound.AdvancePoint(curRightBound.ZeroPoint, startDist); endPoint = curRightBound.AdvancePoint(startPoint, endDist); curRightBound = curRightBound.SubPath(startPoint, endPoint); if (cancelled) return; Services.UIService.PushRelativePath(subPath, curTimestamp, "subpath"); Services.UIService.PushRelativePath(curLeftBound, curTimestamp, "left bound"); Services.UIService.PushRelativePath(curRightBound, curTimestamp, "right bound"); // run a path smoothing iteration lock (this) { planner = new PathPlanner(); } //////////////////////////////////////////////////////////////////////////////////////////////////// // start of obstacle manager - hik bool obstacleManagerEnable = true; PathPlanner.SmoothingResult result; if (obstacleManagerEnable == true) { // generate fake obstacles (for simulation testing only) double obsSize = 10.5 / 2; List<Coordinates> obstaclePoints = new List<Coordinates>(); List<Obstacle> obstacleClusters = new List<Obstacle>(); // fake left obstacles (for simulation only) int totalLeftObstacles = Math.Min(0, curLeftBound.Count - 1); for (int i = 0; i < totalLeftObstacles; i++) { obstaclePoints.Clear(); obstaclePoints.Add(curLeftBound[i] + new Coordinates(obsSize, obsSize)); obstaclePoints.Add(curLeftBound[i] + new Coordinates(obsSize, -obsSize)); obstaclePoints.Add(curLeftBound[i] + new Coordinates(-obsSize, -obsSize)); obstaclePoints.Add(curLeftBound[i] + new Coordinates(-obsSize, obsSize)); obstacleClusters.Add(new Obstacle()); obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints); } // fake right obstacles (for simulation only) int totalRightObstacles = Math.Min(0, curRightBound.Count - 1); for (int i = 0; i < totalRightObstacles; i++) { obstaclePoints.Clear(); obstaclePoints.Add(curRightBound[i] + new Coordinates(obsSize, obsSize)); obstaclePoints.Add(curRightBound[i] + new Coordinates(obsSize, -obsSize)); obstaclePoints.Add(curRightBound[i] + new Coordinates(-obsSize, -obsSize)); obstaclePoints.Add(curRightBound[i] + new Coordinates(-obsSize, obsSize)); obstacleClusters.Add(new Obstacle()); obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints); } // fake center obstacles (for simulation only) int totalCenterObstacles = Math.Min(0, subPath.Count - 1); for (int i = 2; i < totalCenterObstacles; i++) { obstaclePoints.Clear(); obstaclePoints.Add(subPath[i] + new Coordinates(obsSize, obsSize)); obstaclePoints.Add(subPath[i] + new Coordinates(obsSize, -obsSize)); obstaclePoints.Add(subPath[i] + new Coordinates(-obsSize, -obsSize)); obstaclePoints.Add(subPath[i] + new Coordinates(-obsSize, obsSize)); obstacleClusters.Add(new Obstacle()); obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints); } obstaclePoints.Clear(); obstaclePoints.Add(new Coordinates(10000, 10000)); obstaclePoints.Add(new Coordinates(10000, 10001)); obstaclePoints.Add(new Coordinates(10001, 10000)); obstacleClusters.Add(new Obstacle()); obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints); obstaclePoints.Clear(); obstaclePoints.Add(new Coordinates(1000, 1000)); obstaclePoints.Add(new Coordinates(1000, 1001)); obstaclePoints.Add(new Coordinates(1001, 1000)); obstacleClusters.Add(new Obstacle()); obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints); obstaclePoints.Clear(); obstaclePoints.Add(new Coordinates(-1000, -1000)); obstaclePoints.Add(new Coordinates(-1000, -1001)); obstaclePoints.Add(new Coordinates(-1001, -1000)); obstacleClusters.Add(new Obstacle()); obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints); foreach (Obstacle obs in obstacleClusters) { obs.cspacePolygon = new Polygon(obs.obstaclePolygon.points); } // find obstacle path and left/right classification LinePath obstaclePath = new LinePath(); //Boolean successFlag; //double laneWidthAtPathEnd = 10.0; //#warning this currently doesn't work /*obstacleManager.ProcessObstacles(subPath, new LinePath[] { curLeftBound }, new LinePath[] { curRightBound }, obstacleClusters, laneWidthAtPathEnd, out obstaclePath, out successFlag); */ // prepare left and right lane bounds double laneMinSpacing = 0.1; double laneDesiredSpacing = 0.5; double laneAlphaS = 10000; List<Boundary> leftBounds = new List<Boundary>(); List<Boundary> rightBounds = new List<Boundary>(); leftBounds.Add(new Boundary(curLeftBound, laneMinSpacing, laneDesiredSpacing, laneAlphaS)); rightBounds.Add(new Boundary(curRightBound, laneMinSpacing, laneDesiredSpacing, laneAlphaS)); // sort out obstacles as left and right double obstacleMinSpacing = 0.1; double obstacleDesiredSpacing = 1.0; double obstacleAlphaS = 10000; Boundary bound; int totalObstacleClusters = obstacleClusters.Count; for (int i = 0; i < totalObstacleClusters; i++) { if (obstacleClusters[i].avoidanceStatus == AvoidanceStatus.Left) { // obstacle cluster is to the left of obstacle path bound = new Boundary(obstacleClusters[i].obstaclePolygon.points, obstacleMinSpacing, obstacleDesiredSpacing, obstacleAlphaS, true); bound.CheckFrontBumper = true; leftBounds.Add(bound); } else if (obstacleClusters[i].avoidanceStatus == AvoidanceStatus.Right) { // obstacle cluster is to the right of obstacle path bound = new Boundary(obstacleClusters[i].obstaclePolygon.points, obstacleMinSpacing, obstacleDesiredSpacing, obstacleAlphaS, true); bound.CheckFrontBumper = true; rightBounds.Add(bound); } else { // obstacle cluster is outside grid, hence ignore obstacle cluster } } Stopwatch stopwatch = new Stopwatch(); stopwatch.Start(); // PlanPath function call with obstacle path and obstacles result = planner.PlanPath(obstaclePath, obstaclePath, leftBounds, rightBounds, 0, maxSpeed, vs.speed, null, curTimestamp, false); stopwatch.Stop(); Console.WriteLine("============================================================"); Console.WriteLine("With ObstacleManager - Planner - Elapsed (ms): {0}", stopwatch.ElapsedMilliseconds); Console.WriteLine("============================================================"); } else { Stopwatch stopwatch = new Stopwatch(); stopwatch.Start(); // original PlanPath function call result = planner.PlanPath(subPath, curLeftBound, curRightBound, 0, maxSpeed, vs.speed, null, curTimestamp); stopwatch.Stop(); Console.WriteLine("============================================================"); Console.WriteLine("Without ObstacleManager - Planner - Elapsed (ms): {0}", stopwatch.ElapsedMilliseconds); Console.WriteLine("============================================================"); } // end of obstacle manager - hik //////////////////////////////////////////////////////////////////////////////////////////////////// //PathPlanner.PlanningResult result = planner.PlanPath(subPath, curLeftBound, curRightBound, 0, maxSpeed, vs.speed, null, curTimestamp); //SmoothedPath path = new SmoothedPath(pathTime); lock (this) { planner = null; } if (cancelled) return; if (result.result == UrbanChallenge.PathSmoothing.SmoothResult.Sucess) { // start tracking the path Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetSmoothedPathVelocityCommand(result.path)); //Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetConstantSteeringConstantSpeedCommand(-.5, 2)); /*TrackingCommand cmd = new TrackingCommand( new FeedbackSpeedCommandGenerator(new ConstantSpeedGenerator(2, null)), new SinSteeringCommandGenerator(), true); Services.TrackingManager.QueueCommand(cmd);*/ // send the path's we're tracking to the UI Services.UIService.PushRelativePath(result.path, curTimestamp, "smoothed path"); cancelled = true; } }
public static void SmoothAndTrack(LinePath basePath, bool useTargetPath, IList<LinePath> leftBounds, IList<LinePath> rightBounds, double maxSpeed, double? endingHeading, bool endingOffsetBound, CarTimestamp curTimestamp, bool doAvoidance, SpeedCommand speedCommand, CarTimestamp behaviorTimestamp, string commandLabel, ref bool cancelled, ref LinePath previousSmoothedPath, ref CarTimestamp previousSmoothedPathTimestamp, ref double? approachSpeed) { // if we're in listen mode, just return for now if (OperationalBuilder.BuildMode == BuildMode.Listen) { return; } PathPlanner.PlanningResult result; double curSpeed = Services.StateProvider.GetVehicleState().speed; LinePath targetPath = new LinePath(); double initialHeading = 0; // get the part we just used to make a prediction if (useTargetPath && previousSmoothedPath != null) { //targetPath = previousSmoothedPath.Transform(Services.RelativePose.GetTransform(previousSmoothedPathTimestamp, curTimestamp)); // interpolate the path with a smoothing spline //targetPath = targetPath.SplineInterpolate(0.05); //Services.UIService.PushRelativePath(targetPath, curTimestamp, "prediction path2"); // calculate the point speed*dt ahead /*double lookaheadDist = curSpeed*0.20; if (lookaheadDist > 0.1) { LinePath.PointOnPath pt = targetPath.AdvancePoint(targetPath.ZeroPoint, lookaheadDist); // get the heading initialHeading = targetPath.GetSegment(pt.Index).UnitVector.ArcTan; // adjust the base path start point to be the predicted location basePath[0] = pt.Location; // get the part we just used to make a prediction predictionPath = targetPath.SubPath(targetPath.ZeroPoint, pt); // push to the UI Services.UIService.PushRelativePath(predictionPath, curTimestamp, "prediction path"); //basePath[0] = new Coordinates(lookaheadDist, 0); Services.UIService.PushRelativePath(basePath, curTimestamp, "subpath2"); Services.Dataset.ItemAs<double>("initial heading").Add(initialHeading, curTimestamp); // calculate a piece of the sub path //targetPath = targetPath.SubPath(targetPath.ZeroPoint, 7); }*/ // get the tracking manager to predict stuff like whoa AbsolutePose absPose; OperationalVehicleState vehicleState; Services.TrackingManager.ForwardPredict(out absPose, out vehicleState); // insert the stuff stuff basePath[0] = absPose.xy; initialHeading = absPose.heading; // start walking down the path until the angle is cool double angle_threshold = 30*Math.PI/180.0; double dist; LinePath.PointOnPath newPoint = new LinePath.PointOnPath(); for (dist = 0; dist < 10; dist += 1) { // get the point advanced from the 2nd point on the base path by dist double distTemp = dist; newPoint = basePath.AdvancePoint(basePath.GetPointOnPath(1), ref distTemp); // check if we're past the end if (distTemp > 0) { break; } // check if the angle is coolness or not double angle = Math.Acos((newPoint.Location-basePath[0]).Normalize().Dot(basePath.GetSegment(newPoint.Index).UnitVector)); if (Math.Acos(angle) < angle_threshold) { break; } } // create a new version of the base path with the stuff section removed basePath = basePath.RemoveBetween(basePath.StartPoint, newPoint); Services.UIService.PushRelativePath(basePath, curTimestamp, "subpath2"); // zero that stuff out targetPath = new LinePath(); } StaticObstacles obstacles = null; // only do the planning is we're in a lane scenario // otherwise, the obstacle grid will be WAY too large if (doAvoidance && leftBounds.Count == 1 && rightBounds.Count == 1) { // get the obstacles predicted to the current timestamp obstacles = Services.ObstaclePipeline.GetProcessedObstacles(curTimestamp); } // start the planning timer Stopwatch planningTimer = Stopwatch.StartNew(); // check if there are any obstacles if (obstacles != null && obstacles.polygons != null && obstacles.polygons.Count > 0) { if (cancelled) return; // we need to do the full obstacle avoidance // execute the obstacle manager LinePath avoidancePath; List<ObstacleManager.ObstacleType> obstacleSideFlags; bool success; Services.ObstacleManager.ProcessObstacles(basePath, leftBounds, rightBounds, obstacles.polygons, out avoidancePath, out obstacleSideFlags, out success); // check if we have success if (success) { // build the boundary lists // start with the lanes List<Boundary> leftSmootherBounds = new List<Boundary>(); List<Boundary> rightSmootherBounds = new List<Boundary>(); double laneMinSpacing = 0.1; double laneDesiredSpacing = 0.1; double laneAlphaS = 0.1; leftSmootherBounds.Add(new Boundary(leftBounds[0], laneMinSpacing, laneDesiredSpacing, laneAlphaS)); rightSmootherBounds.Add(new Boundary(rightBounds[0], laneMinSpacing, laneDesiredSpacing, laneAlphaS)); // sort out obstacles as left and right double obstacleMinSpacing = 0.8; double obstacleDesiredSpacing = 0.8; double obstacleAlphaS = 100; int totalObstacleClusters = obstacles.polygons.Count; for (int i = 0; i < totalObstacleClusters; i++) { if (obstacleSideFlags[i] == ObstacleManager.ObstacleType.Left) { Boundary bound = new Boundary(obstacles.polygons[i], obstacleMinSpacing, obstacleDesiredSpacing, obstacleAlphaS); bound.CheckFrontBumper = true; leftSmootherBounds.Add(bound); } else if (obstacleSideFlags[i] == ObstacleManager.ObstacleType.Right) { Boundary bound = new Boundary(obstacles.polygons[i], obstacleMinSpacing, obstacleDesiredSpacing, obstacleAlphaS); bound.CheckFrontBumper = true; rightSmootherBounds.Add(bound); } } if (cancelled) return; // execute the smoothing PathPlanner planner = new PathPlanner(); planner.Options.alpha_w = 0; planner.Options.alpha_d = 10; planner.Options.alpha_c = 10; result = planner.PlanPath(avoidancePath, targetPath, leftSmootherBounds, rightSmootherBounds, initialHeading, maxSpeed, Services.StateProvider.GetVehicleState().speed, endingHeading, curTimestamp, endingOffsetBound); } else { // mark that we did not succeed result = new PathPlanner.PlanningResult(SmoothResult.Infeasible, null); } } else { if (cancelled) return; // do the path smoothing PathPlanner planner = new PathPlanner(); List<LineList> leftList = new List<LineList>(); foreach (LinePath ll in leftBounds) leftList.Add(ll); List<LineList> rightList = new List<LineList>(); foreach (LinePath rl in rightBounds) rightList.Add(rl); planner.Options.alpha_w = 0; planner.Options.alpha_s = 0.1; planner.Options.alpha_d = 10; planner.Options.alpha_c = 10; result = planner.PlanPath(basePath, targetPath, leftList, rightList, initialHeading, maxSpeed, Services.StateProvider.GetVehicleState().speed, endingHeading, curTimestamp, endingOffsetBound); } planningTimer.Stop(); BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "planning took {0} ms", planningTimer.ElapsedMilliseconds); Services.Dataset.ItemAs<bool>("route feasible").Add(result.result == SmoothResult.Sucess, LocalCarTimeProvider.LocalNow); if (result.result == SmoothResult.Sucess) { // insert the point (-1,0) so we make sure that the zero point during tracking is at the vehicle //Coordinates startingVec = result.path[1].Point-result.path[0].Point; //Coordinates insertPoint = result.path[0].Point-startingVec.Normalize(); //result.path.Insert(0, new OperationalLayer.PathPlanning.PathPoint(insertPoint, maxSpeed)); previousSmoothedPath = new LinePath(result.path); previousSmoothedPathTimestamp = curTimestamp; Services.UIService.PushLineList(previousSmoothedPath, curTimestamp, "smoothed path", true); if (cancelled) return; // we've planned out the path, now build up the command ISpeedGenerator speedGenerator; if (speedCommand is ScalarSpeedCommand) { /*if (result.path.HasSpeeds) { speedGenerator = result.path; } else {*/ speedGenerator = new ConstantSpeedGenerator(maxSpeed, null); //} } else if (speedCommand is StopAtDistSpeedCommand) { StopAtDistSpeedCommand stopCommand = (StopAtDistSpeedCommand)speedCommand; IDistanceProvider distProvider = new TravelledDistanceProvider(behaviorTimestamp, stopCommand.Distance); speedGenerator = new StopSpeedGenerator(distProvider, approachSpeed.Value); BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "stay in lane - remaining stop stop dist {0}", distProvider.GetRemainingDistance()); } else if (speedCommand is StopAtLineSpeedCommand) { IDistanceProvider distProvider = new StoplineDistanceProvider(); speedGenerator = new StopSpeedGenerator(distProvider, approachSpeed.Value); BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "stay in lane - remaining stop stop dist {0}", distProvider.GetRemainingDistance()); } else if (speedCommand == null) { throw new InvalidOperationException("Speed command is null"); } else { throw new InvalidOperationException("Speed command " + speedCommand.GetType().FullName + " is not supported"); } if (cancelled) return; // build up the command TrackingCommand trackingCommand = new TrackingCommand(new FeedbackSpeedCommandGenerator(speedGenerator), new PathSteeringCommandGenerator(result.path), false); trackingCommand.Label = commandLabel; // queue it to execute Services.TrackingManager.QueueCommand(trackingCommand); } }
protected PlanningResult Smooth(bool doAvoidance) { double curSpeed = vs.speed; Services.Dataset.ItemAs<double>("extra spacing").Add(Services.ObstaclePipeline.ExtraSpacing, curTimestamp); Services.Dataset.ItemAs<double>("smoother spacing adjust").Add(smootherSpacingAdjust, curTimestamp); // get the tracking manager to predict stuff like whoa AbsolutePose absPose; OperationalVehicleState vehicleState; double averageCycleTime = Services.BehaviorManager.AverageCycleTime; Services.TrackingManager.ForwardPredict(averageCycleTime, out absPose, out vehicleState); Services.Dataset.ItemAs<double>("planning cycle time").Add(averageCycleTime, LocalCarTimeProvider.LocalNow); settings.initHeading = absPose.heading; smootherBasePath = ReplaceFirstPoint(smootherBasePath, maxSmootherBasePathAdvancePoint, absPose.xy); if (avoidanceBasePath != null && avoidanceBasePath.Count > 0) { avoidanceBasePath = ReplaceFirstPoint(avoidanceBasePath, maxAvoidanceBasePathAdvancePoint, absPose.xy); } else { avoidanceBasePath = smootherBasePath; } if (smootherBasePath.EndPoint.Location.Length > 80) { return OnDynamicallyInfeasible(null, null); } Services.UIService.PushRelativePath(smootherBasePath, curTimestamp, "subpath2"); IList<Obstacle> obstacles = null; if (doAvoidance && Settings.DoAvoidance) { // get the obstacles predicted to the current timestamp ObstacleCollection obs = Services.ObstaclePipeline.GetProcessedObstacles(curTimestamp, Services.BehaviorManager.SAUDILevel); if (obs != null) obstacles = obs.obstacles; } if (extraObstacles != null && extraObstacles.Count > 0) { if (obstacles == null) { obstacles = extraObstacles; } else { foreach (Obstacle obs in extraObstacles) { obstacles.Add(obs); } } } // start the planning timer Stopwatch planningTimer = Stopwatch.StartNew(); List<Boundary> leftSmootherBounds = new List<Boundary>(); List<Boundary> rightSmootherBounds = new List<Boundary>(); leftSmootherBounds.AddRange(leftLaneBounds); rightSmootherBounds.AddRange(rightLaneBounds); leftSmootherBounds.AddRange(additionalLeftBounds); rightSmootherBounds.AddRange(additionalRightBounds); BlockageInstructions blockageInstructions = new BlockageInstructions(); blockageInstructions.smootherInstructions = SmootherInstructions.RunNormalSmoothing; bool pathBlocked = false; // check if there are any obstacles if (obstacles != null && obstacles.Count > 0) { // label the obstacles as ignored if (ignorableObstacles != null) { if (ignorableObstacles.Count == 1 && ignorableObstacles[0] == -1) { for (int i = 0; i < obstacles.Count; i++) { if (obstacles[i].obstacleClass == ObstacleClass.DynamicCarlike || obstacles[i].obstacleClass == ObstacleClass.DynamicStopped) { obstacles[i].ignored = true; } } } else { ignorableObstacles.Sort(); for (int i = 0; i < obstacles.Count; i++) { if (obstacles[i].trackID != -1 && ignorableObstacles.BinarySearch(obstacles[i].trackID) >= 0) { obstacles[i].ignored = true; } } } } // we need to do the full obstacle avoidance // execute the obstacle manager LinePath avoidancePath; bool success; List<LinePath> shiftedLeftBounds = new List<LinePath>(leftLaneBounds.Count); foreach (Boundary bnd in leftLaneBounds) { LinePath lp = (LinePath)bnd.Coords; if (lp.Count >= 2) { shiftedLeftBounds.Add(lp.ShiftLateral(-(TahoeParams.T)/2.0+0.25)); } } List<LinePath> shiftedRightBounds = new List<LinePath>(rightLaneBounds.Count); foreach (Boundary bnd in rightLaneBounds) { LinePath lp = (LinePath)bnd.Coords; if (lp.Count >= 2) { shiftedRightBounds.Add(lp.ShiftLateral((TahoeParams.T)/2.0-0.25)); } } try { Services.ObstacleManager.ProcessObstacles(avoidanceBasePath, shiftedLeftBounds, shiftedRightBounds, obstacles, laneWidthAtPathEnd, reverseGear, sparse, out avoidancePath, out success); } catch (OutOfMemoryException ex) { Console.WriteLine("out of memory exception at " + ex.StackTrace); return OnDynamicallyInfeasible(obstacles, null); } if (!success) { // build out the distance DetermineBlockageDistancesAndDeceleration(obstacles, avoidanceBasePath); // call the on blocked stuff blockageInstructions = OnPathBlocked(obstacles); pathBlocked = blockageInstructions.pathBlocked; } if (blockageInstructions.smootherInstructions == SmootherInstructions.RunNormalSmoothing || blockageInstructions.smootherInstructions == SmootherInstructions.UseSmootherSpeedCommands) { // build the boundary lists // sort out obstacles as left and right double obstacleAlphaS = 100; int totalObstacleClusters = obstacles.Count; for (int i = 0; i < totalObstacleClusters; i++) { if (obstacles[i].ignored) continue; double minSpacing = Math.Max(obstacles[i].minSpacing + smootherSpacingAdjust, 0); double desSpacing = Math.Max(obstacles[i].desSpacing + smootherSpacingAdjust, 0); if (obstacles[i].avoidanceStatus == AvoidanceStatus.Left) { Boundary bound = new Boundary(obstacles[i].AvoidancePolygon, minSpacing, desSpacing, obstacleAlphaS); bound.CheckFrontBumper = true; leftSmootherBounds.Add(bound); } else if (obstacles[i].avoidanceStatus == AvoidanceStatus.Right) { Boundary bound = new Boundary(obstacles[i].AvoidancePolygon, minSpacing, desSpacing, obstacleAlphaS); bound.CheckFrontBumper = true; rightSmootherBounds.Add(bound); } } } // we could possibly replace the smoother base with the avoidance path or we can adjust the smoother base path // appropriately if (success && useAvoidancePath) { smootherBasePath = avoidancePath; } Services.UIService.PushLineList(avoidancePath, curTimestamp, "avoidance path", true); } PlanningResult planningResult = null; if (blockageInstructions.smootherInstructions == SmootherInstructions.RunNormalSmoothing || blockageInstructions.smootherInstructions == SmootherInstructions.UseSmootherSpeedCommands || (blockageInstructions.smootherInstructions == SmootherInstructions.UsePreviousPath && prevSmoothedPath == null)) { // initialize settings that we're making easier for the derived classes settings.basePath = smootherBasePath; settings.targetPaths = smootherTargetPaths; settings.leftBounds = leftSmootherBounds; settings.rightBounds = rightSmootherBounds; settings.startSpeed = curSpeed; settings.timestamp = curTimestamp; PathPlanner.SmoothingResult result = planner.PlanPath(settings); if (result.result != SmoothResult.Sucess) { planningResult = OnDynamicallyInfeasible(obstacles, result.details); if (blockageInstructions.speedCommand != null) { planningResult.speedCommandGenerator = blockageInstructions.speedCommand; } } else { // build out the command planningResult = new PlanningResult(); planningResult.pathBlocked = pathBlocked; planningResult.smoothedPath = result.path; if (blockageInstructions.smootherInstructions == SmootherInstructions.UseSmootherSpeedCommands) { planningResult.speedCommandGenerator = new FeedbackSpeedCommandGenerator(result.path); } else if (blockageInstructions.speedCommand != null) { planningResult.speedCommandGenerator = blockageInstructions.speedCommand; } if (blockageInstructions.smootherInstructions == SmootherInstructions.UseCommandGenerator) { planningResult.steeringCommandGenerator = blockageInstructions.steeringCommand; } else { planningResult.steeringCommandGenerator = new PathSteeringCommandGenerator(result.path); } } } else if (blockageInstructions.smootherInstructions == SmootherInstructions.UsePreviousPath){ // transform the previously smoothed path into the current interval RelativeTransform transform = Services.RelativePose.GetTransform(prevSmoothedPathTimestamp, curTimestamp); SmoothedPath prevPath = new SmoothedPath(curTimestamp, prevSmoothedPath.Transform(transform), null); planningResult = new PlanningResult(); planningResult.speedCommandGenerator = blockageInstructions.speedCommand; planningResult.smoothedPath = prevPath; planningResult.pathBlocked = pathBlocked; planningResult.steeringCommandGenerator = new PathSteeringCommandGenerator(prevPath); } else if (blockageInstructions.smootherInstructions == SmootherInstructions.UseCommandGenerator) { planningResult = new PlanningResult(); planningResult.speedCommandGenerator = blockageInstructions.speedCommand; planningResult.steeringCommandGenerator = blockageInstructions.steeringCommand; planningResult.pathBlocked = pathBlocked; planningResult.smoothedPath = null; } planningTimer.Stop(); BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "planning took {0} ms", planningTimer.ElapsedMilliseconds); planningResult.commandLabel = blockageInstructions.commandLabel; if (!planningResult.pathBlocked && !planningResult.dynamicallyInfeasible) { OnSmoothSuccess(ref planningResult); } return planningResult; }