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;
}
