public SmoothingResult(SmoothResult result, SmoothedPath path, AvoidanceDetails details)
{
this.result = result;
this.path = path;
this.details = details;
}
public static TrackingCommand GetSmoothedPathVelocityCommand(SmoothedPath path)
{
return new TrackingCommand(new FeedbackSpeedCommandGenerator(path), new PathSteeringCommandGenerator(path), false);
}
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);
}*/
}
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);
* }*/
}
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;
}
protected virtual PlanningResult OnDynamicallyInfeasible(IList<Obstacle> obstacles, AvoidanceDetails details, bool sendBlockage)
{
double scalarSpeed = -1;
if (speedCommand is ScalarSpeedCommand) {
scalarSpeed = ((ScalarSpeedCommand)speedCommand).Speed;
}
// see if we can figure out a stop distance based on stuff crossing
double stopDist = double.NaN;
try {
if (details != null) {
double totalDist = 0;
int numBounds = details.smoothingDetails.leftBounds.Length;
for (int i = 0; i < numBounds; i++) {
// left is less than right, we can't make it through
if (details.smoothingDetails.leftBounds[i].deviation < details.smoothingDetails.rightBounds[i].deviation) {
stopDist = totalDist;
break;
}
totalDist += 0.5;
}
}
}
catch (Exception) {
}
if (vs.IsStopped && scalarSpeed != 0 && sendBlockage && !Services.BehaviorManager.TestMode) {
if (lastDynInfeasibleTime == null) {
lastDynInfeasibleTime = HighResDateTime.Now;
}
else if (HighResDateTime.Now - lastDynInfeasibleTime.Value > TimeSpan.FromSeconds(2)) {
// send a completion report with the error
bool stopTooClose = stopDist < TahoeParams.VL || double.IsNaN(stopDist);
CompletionReport report = new TrajectoryBlockedReport(UrbanChallenge.Behaviors.CompletionReport.CompletionResult.Stopped, stopDist, BlockageType.Unknown, -1, stopTooClose || DetermineReverseRecommended(obstacles), Services.BehaviorManager.CurrentBehaviorType);
ForwardCompletionReport(report);
}
}
else {
lastDynInfeasibleTime = null;
}
if (Services.BehaviorManager.TestMode) {
bool stopTooClose = stopDist < TahoeParams.VL || double.IsNaN(stopDist);
CompletionReport report = new TrajectoryBlockedReport(UrbanChallenge.Behaviors.CompletionReport.CompletionResult.Stopped, stopDist, BlockageType.Unknown, -1, stopTooClose || DetermineReverseRecommended(obstacles), Services.BehaviorManager.CurrentBehaviorType);
ForwardCompletionReport(report);
}
stopDist -= 2;
if (stopDist < 0.01) {
stopDist = 0.01;
}
double nomDecel = 3;
if (scalarSpeed == 0) {
nomDecel = 4;
}
// nominal stopping acceleration is 3 m/s^2
double nomStoppingDist = vs.speed*vs.speed/(2*nomDecel);
if (double.IsNaN(stopDist) || stopDist > nomStoppingDist) {
stopDist = nomStoppingDist;
}
// figure out the target deceleration
double targetDecel = vs.speed*vs.speed/(2*stopDist);
PlanningResult result = new PlanningResult();
// figure out if arbiter is already stopping shorter
double commandedStopDist = GetSpeedCommandStopDistance();
if (!double.IsPositiveInfinity(commandedStopDist) && commandedStopDist < stopDist) {
// don't need to do anything, we're already stopping appropriately
result.speedCommandGenerator = null;
}
else if (vs.IsStopped) {
// just hold the brakes with the standard pressure
result.speedCommandGenerator = new ConstantSpeedCommandGenerator(0, TahoeParams.brake_hold+1);
}
else {
// do a constant deceleration profile
result.speedCommandGenerator = new FeedbackSpeedCommandGenerator(new ConstantAccelSpeedGenerator(-targetDecel));
}
if (prevSmoothedPath != null) {
RelativeTransform transform = Services.RelativePose.GetTransform(prevSmoothedPathTimestamp, curTimestamp);
SmoothedPath smoothedPath = new SmoothedPath(curTimestamp, prevSmoothedPath.Transform(transform), null);
result.steeringCommandGenerator = new PathSteeringCommandGenerator(smoothedPath);
result.smoothedPath = smoothedPath;
}
else {
result.steeringCommandGenerator = new ConstantSteeringCommandGenerator(null, null, false);
}
result.dynamicallyInfeasible = true;
result.commandLabel = "dynamically infeasible";
return result;
}
private double PostProcessSpeed(SmoothedPath path)
{
const double a_lat_max = 1.5;
const double lat_target_decel = 1;
double maxSpeed = 20;
// return an astronomical speed if there are no curvatures
if (path.Count < 3)
return maxSpeed;
// calculate the curvature at each point
// we want the lateral acceleration to be no more than a_lat_max, so calculate the speed we would need to be going now achieve that deceleration
double dist = 0;
for (int i = 1; i < path.Count-1; i++) {
dist += path[i-1].DistanceTo(path[i]);
// lateral acceleration = v^2*curvature
double curvature = path.GetCurvature(i);
double desiredSpeed = Math.Sqrt(a_lat_max/Math.Abs(curvature));
double desiredMaxSpeed = Math.Sqrt(desiredSpeed*desiredSpeed + 2*lat_target_decel*dist);
if (desiredMaxSpeed < maxSpeed) {
maxSpeed = desiredMaxSpeed;
}
}
return maxSpeed;
}
