protected override void OnSmoothSuccess(ref PlanningResult result)
{
if (!(result.pathBlocked || result.dynamicallyInfeasible) && !reverseGear) {
// check the path, see if the first segment goes 180 off
if (result.smoothedPath != null && result.smoothedPath.Count >= 2 && Math.Abs(result.smoothedPath.GetSegment(0).UnitVector.ArcTan) > 90*Math.PI/180) {
result = OnDynamicallyInfeasible(null, null, true);
Console.WriteLine("turn is 180 deg off, returning infeasible");
return;
}
}
base.OnSmoothSuccess(ref result);
}
private void ArcVoteZone(Coordinates goalPoint, List<Obstacle> perimeterObstacles)
{
List<Polygon> perimeterPolys = new List<Polygon>();
foreach (Obstacle obs in perimeterObstacles) {
perimeterPolys.Add(obs.cspacePolygon);
}
PlanningResult result;
ISteeringCommandGenerator steeringCommand = ZoneArcVoting.SparcVote(ref prevCurvature, goalPoint, perimeterPolys);
if (steeringCommand == null) {
result = OnDynamicallyInfeasible(null, null);
}
else {
result = new PlanningResult();
result.commandLabel = command_label;
result.steeringCommandGenerator = steeringCommand;
}
settings.maxSpeed = recommendedSpeed.Speed;
Track(result, command_label);
}
public override void Process(object param)
{
// inform the base that we're beginning processing
if (!BeginProcess()) {
return;
}
// check if we were given a parameter
if (param != null && param is StayInLaneBehavior) {
HandleBehavior((StayInLaneBehavior)param);
}
if (reverseGear) {
ProcessReverse();
return;
}
// figure out the planning distance
double planningDistance = GetPlanningDistance();
if (sparse && planningDistance > 10) {
planningDistance = 10;
}
double? boundStartDistMin = null;
double? boundEndDistMax = null;
if (laneID != null && Services.RoadNetwork != null) {
ArbiterLane lane = Services.RoadNetwork.ArbiterSegments[laneID.SegmentId].Lanes[laneID];
AbsolutePose pose = Services.StateProvider.GetAbsolutePose();
ArbiterLanePartition partition = lane.GetClosestPartition(pose.xy);
LinePath partitionPath = partition.UserPartitionPath;
LinePath.PointOnPath closestPoint = partitionPath.GetClosestPoint(pose.xy);
double remainingDist = planningDistance;
double totalDist = partitionPath.DistanceBetween(closestPoint, partitionPath.EndPoint);
remainingDist -= totalDist;
if (sparse) {
// walk ahead and determine where sparsity ends
bool nonSparseFound = false;
while (remainingDist > 0) {
// get the next partition
partition = partition.Final.NextPartition;
if (partition == null)
break;
if (partition.Type != PartitionType.Sparse) {
nonSparseFound = true;
break;
}
else {
double dist = partition.UserPartitionPath.PathLength;
totalDist += dist;
remainingDist -= dist;
}
}
if (nonSparseFound) {
boundStartDistMin = totalDist;
}
}
else {
// determine if there is a sparse segment upcoming
bool sparseFound = false;
while (remainingDist > 0) {
partition = partition.Final.NextPartition;
if (partition == null) {
break;
}
if (partition.Type == PartitionType.Sparse) {
sparseFound = true;
break;
}
else {
double dist = partition.Length;
totalDist += dist;
remainingDist -= dist;
}
}
if (sparseFound) {
boundEndDistMax = totalDist;
sparse = true;
}
}
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "in stay in lane, planning distance {0}", planningDistance);
// update the rndf path
RelativeTransform relTransfrom = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
LinePath curRndfPath = rndfPath.Transform(relTransfrom);
ILaneModel centerLaneModel = Services.RoadModelProvider.GetLaneModel(curRndfPath, rndfPathWidth + extraWidth, curTimestamp, numLanesLeft, numLanesRight);
double avoidanceExtra = sparse ? 5 : 7.5;
LinePath centerLine, leftBound, rightBound;
if (boundEndDistMax != null || boundStartDistMin != null) {
LinearizeStayInLane(centerLaneModel, planningDistance+avoidanceExtra, null, boundEndDistMax, boundStartDistMin, null, curTimestamp, out centerLine, out leftBound, out rightBound);
}
else {
LinearizeStayInLane(centerLaneModel, planningDistance+avoidanceExtra, curTimestamp, out centerLine, out leftBound, out rightBound);
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "in stay in lane, center line count: {0}, left bound count: {1}, right bound count: {2}", centerLine.Count, leftBound.Count, rightBound.Count);
// calculate time to collision of opposing obstacle if it exists
LinePath targetLine = centerLine;
double targetWeight = default_lane_alpha_w;
if (sparse) {
targetWeight = 0.00000025;
}
else if (oncomingVehicleExists) {
double shiftDist = -(TahoeParams.T + 1);
targetLine = centerLine.ShiftLateral(shiftDist);
targetWeight = 0.01;
}
else if (opposingLaneVehicleExists) {
double timeToCollision = opposingLaneVehicleDist/(Math.Abs(vs.speed)+Math.Abs(opposingLaneVehicleSpeed));
Services.Dataset.ItemAs<double>("time to collision").Add(timeToCollision, curTimestamp);
if (timeToCollision < 5) {
// shift to the right
double shiftDist = -TahoeParams.T/2.0;
targetLine = centerLine.ShiftLateral(shiftDist);
}
}
// set up the planning
AddTargetPath(targetLine, targetWeight);
if (!sparse || boundStartDistMin != null || boundEndDistMax != null) {
AddLeftBound(leftBound, true);
if (!oncomingVehicleExists) {
AddRightBound(rightBound, false);
}
}
double targetDist = Math.Max(centerLine.PathLength-avoidanceExtra, planningDistance);
smootherBasePath = centerLine.SubPath(centerLine.StartPoint, targetDist);
maxSmootherBasePathAdvancePoint = smootherBasePath.EndPoint;
double extraDist = (planningDistance+avoidanceExtra)-centerLine.PathLength;
extraDist = Math.Min(extraDist, 5);
if (extraDist > 0) {
centerLine.Add(centerLine[centerLine.Count-1] + centerLine.EndSegment.Vector.Normalize(extraDist));
}
avoidanceBasePath = centerLine;
Services.UIService.PushLineList(centerLine, curTimestamp, "subpath", true);
Services.UIService.PushLineList(leftBound, curTimestamp, "left bound", true);
Services.UIService.PushLineList(rightBound, curTimestamp, "right bound", true);
// get the overall max speed looking forward from our current point
settings.maxSpeed = GetMaxSpeed(curRndfPath, curRndfPath.AdvancePoint(curRndfPath.ZeroPoint, vs.speed*TahoeParams.actuation_delay));
// get the max speed at the end point
settings.maxEndingSpeed = GetMaxSpeed(curRndfPath, curRndfPath.AdvancePoint(curRndfPath.ZeroPoint, planningDistance));
useAvoidancePath = false;
if (sparse) {
// limit to 5 mph
laneWidthAtPathEnd = 20;
pathAngleCheckMax = 50;
pathAngleMax = 5 * Math.PI / 180.0;
settings.maxSpeed = Math.Min(settings.maxSpeed, 2.2352);
maxAvoidanceBasePathAdvancePoint = avoidanceBasePath.AdvancePoint(avoidanceBasePath.EndPoint, -2);
//maxSmootherBasePathAdvancePoint = smootherBasePath.AdvancePoint(smootherBasePath.EndPoint, -2);
LinePath leftEdge = RoadEdge.GetLeftEdgeLine();
LinePath rightEdge = RoadEdge.GetRightEdgeLine();
if (leftEdge != null) {
leftLaneBounds.Add(new Boundary(leftEdge, 0.1, 1, 100, false));
}
if (rightEdge != null) {
rightLaneBounds.Add(new Boundary(rightEdge, 0.1, 1, 100, false));
}
PlanningResult result = new PlanningResult();
ISteeringCommandGenerator commandGenerator = SparseArcVoting.SparcVote(ref prevCurvature, goalPoint);
if (commandGenerator == null) {
// we have a block, report dynamically infeasible
result = OnDynamicallyInfeasible(null, null);
}
else {
result.steeringCommandGenerator = commandGenerator;
result.commandLabel = commandLabel;
}
Track(result, commandLabel);
return;
}
else if (oncomingVehicleExists) {
laneWidthAtPathEnd = 10;
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "max speed set to {0}", settings.maxSpeed);
disablePathAngleCheck = false;
SmoothAndTrack(commandLabel, true);
}
protected override void OnSmoothSuccess(ref PlanningResult result)
{
if (sparse) {
if (Services.ObstaclePipeline.ExtraSpacing < 1) {
Services.ObstaclePipeline.ExtraSpacing = Math.Min(1, Services.ObstaclePipeline.ExtraSpacing + 0.02);
}
if (smootherSpacingAdjust < 0) {
smootherSpacingAdjust = Math.Min(0, smootherSpacingAdjust + 0.01);
}
}
base.OnSmoothSuccess(ref result);
}
private PlanningPhase PlanStartingLane(LinePath curStartingLane, LinePath curEndingLane)
{
ILaneModel startingLaneModel = Services.RoadModelProvider.GetLaneModel(curStartingLane, startingLaneWidth + extraWidth, curTimestamp, startingLanesLeft, startingLanesRight);
// figure out where we are along the starting lane path
LinePath.PointOnPath startingLanePoint = curStartingLane.ZeroPoint;
// calculate the planning distance
double planningDistance = GetPlanningDistance();
if (sparse && planningDistance > 10) {
planningDistance = 10;
}
// get the distance to the end point of the lane from our current point
double remainingDistance = curStartingLane.DistanceBetween(startingLanePoint, curStartingLane.EndPoint);
double? boundStartDistMin;
double? boundEndDistMax;
DetermineSparseStarting(planningDistance, out boundStartDistMin, out boundEndDistMax);
double avoidanceExtra = sparse ? 5 : 7.5;
// linearize the lane model
LinePath centerLine, leftBound, rightBound;
if (boundEndDistMax != null || boundStartDistMin != null) {
if (boundEndDistMax != null) {
boundEndDistMax = Math.Min(boundEndDistMax.Value, remainingDistance - starting_lane_trim_dist);
}
else {
boundEndDistMax = remainingDistance - starting_lane_trim_dist;
}
LinearizeStayInLane(startingLaneModel, planningDistance + avoidanceExtra, null, boundEndDistMax, boundStartDistMin, null, curTimestamp, out centerLine, out leftBound, out rightBound);
}
else {
LinearizeStayInLane(startingLaneModel, planningDistance + avoidanceExtra, null, remainingDistance - starting_lane_trim_dist, null, curTimestamp,
out centerLine, out leftBound, out rightBound);
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "in stay in lane, center line count: {0}, left bound count: {1}, right bound count: {2}", centerLine.Count, leftBound.Count, rightBound.Count);
// add to the planning setup
avoidanceBasePath = centerLine;
double targetDist = Math.Max(centerLine.PathLength-avoidanceExtra, planningDistance);
smootherBasePath = centerLine.SubPath(centerLine.StartPoint, targetDist);
// calculate time to collision of opposing obstacle if it exists
LinePath targetLine = centerLine;
double targetWeight = default_lane_alpha_w;
if (sparse) {
targetWeight = 0.00000025;
}
else if (oncomingVehicleExists) {
double shiftDist = -(TahoeParams.T + 1);
targetLine = centerLine.ShiftLateral(shiftDist);
targetWeight = 0.01;
}
else if (opposingLaneVehicleExists) {
double timeToCollision = opposingLaneVehicleDist/(Math.Abs(vs.speed)+Math.Abs(opposingLaneVehicleSpeed));
Services.Dataset.ItemAs<double>("time to collision").Add(timeToCollision, curTimestamp);
if (timeToCollision < 5) {
// shift to the right
double laneWidth = startingLaneModel.Width;
double shiftDist = -Math.Min(TahoeParams.T/2.0, laneWidth/2.0 - TahoeParams.T/2.0);
targetLine = centerLine.ShiftLateral(shiftDist);
}
}
// set up the planning
AddTargetPath(targetLine, targetWeight);
if (!sparse || boundStartDistMin != null || boundEndDistMax != null) {
// add the lane bounds
AddLeftBound(leftBound, true);
if (!oncomingVehicleExists) {
AddRightBound(rightBound, false);
}
}
Services.UIService.PushLineList(centerLine, curTimestamp, "subpath", true);
Services.UIService.PushLineList(leftBound, curTimestamp, "left bound", true);
Services.UIService.PushLineList(rightBound, curTimestamp, "right bound", true);
// calculate max speed for the path over the next 120 meters
double advanceDist = speed_path_advance_dist;
LinePath.PointOnPath startingLaneEndPoint = curStartingLane.AdvancePoint(startingLanePoint, ref advanceDist);
LinePath combinedPath = curStartingLane.SubPath(startingLanePoint, startingLaneEndPoint);
Coordinates endingLaneFirstPoint = curEndingLane[0];
Coordinates startingLaneLastPoint = curStartingLane.EndPoint.Location;
double intersectionDist = startingLaneLastPoint.DistanceTo(endingLaneFirstPoint);
// add a portion of the ending lane of the appropriate length
LinePath.PointOnPath endingLanePoint = curEndingLane.AdvancePoint(curEndingLane.StartPoint, Math.Max(advanceDist - intersectionDist, 5));
int endIndex = endingLanePoint.Index + 1;
combinedPath.AddRange(curEndingLane.GetSubpathEnumerator(0, endIndex));
// add to the planning setup
settings.maxSpeed = GetMaxSpeed(combinedPath, combinedPath.StartPoint);
settings.maxEndingSpeed = GetMaxSpeed(combinedPath, combinedPath.AdvancePoint(combinedPath.StartPoint, planningDistance));
if (sparse) {
// limit to 5 mph
laneWidthAtPathEnd = 20;
pathAngleCheckMax = 50;
pathAngleMax = 5 * Math.PI / 180.0;
settings.maxSpeed = Math.Min(settings.maxSpeed, 2.2352);
LinePath leftEdge = RoadEdge.GetLeftEdgeLine();
LinePath rightEdge = RoadEdge.GetRightEdgeLine();
if (leftEdge != null) {
leftLaneBounds.Add(new Boundary(leftEdge, 0.1, 1, 100, false));
}
if (rightEdge != null) {
rightLaneBounds.Add(new Boundary(rightEdge, 0.1, 1, 100, false));
}
maxAvoidanceBasePathAdvancePoint = avoidanceBasePath.AdvancePoint(avoidanceBasePath.EndPoint, -2);
//maxSmootherBasePathAdvancePoint = smootherBasePath.AdvancePoint(smootherBasePath.EndPoint, -2);
PlanningResult result = new PlanningResult();
ISteeringCommandGenerator commandGenerator = SparseArcVoting.SparcVote(ref prevCurvature, goalPoint);
if (commandGenerator == null) {
// we have a block, report dynamically infeasible
result = OnDynamicallyInfeasible(null, null);
}
else {
result.steeringCommandGenerator = commandGenerator;
result.commandLabel = commandLabel;
}
Track(result, commandLabel);
if (planningDistance > remainingDistance) {
return PlanningPhase.BeginEnteringTurn;
}
else {
return PlanningPhase.StartingLane;
}
}
else if (oncomingVehicleExists) {
laneWidthAtPathEnd = 10;
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "max speed set to {0}", settings.maxSpeed);
SmoothAndTrack(commandLabel, true);
if (cancelled) return PlanningPhase.StartingLane;
// check if we need to transition to the turn phase for the next iteration
// if the planning distance is greater than the remaining distance, i.e. we want to plan into the intersection, then
// we want to partially run the turn behavior next iteration
if (planningDistance > remainingDistance) {
return PlanningPhase.BeginEnteringTurn;
}
else {
return PlanningPhase.StartingLane;
}
}
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 void OnSmoothSuccess(ref PlanningResult result)
{
lastDynInfeasibleTime = null;
lastBlockageTime = null;
}
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;
}
protected void Track(PlanningResult planningResult, string commandLabel)
{
Services.Dataset.ItemAs<bool>("route feasible").Add(!planningResult.pathBlocked, LocalCarTimeProvider.LocalNow);
if (planningResult.smoothedPath != null && !planningResult.dynamicallyInfeasible) {
prevSmoothedPath = new LinePath(planningResult.smoothedPath);
prevSmoothedPathTimestamp = curTimestamp;
}
if (planningResult.smoothedPath != null) {
Services.UIService.PushLineList(prevSmoothedPath, curTimestamp, "smoothed path", true);
}
if (cancelled) return;
ISpeedCommandGenerator speedCommandGenerator = planningResult.speedCommandGenerator;
ISteeringCommandGenerator steeringCommandGenerator = planningResult.steeringCommandGenerator;
if (speedCommandGenerator == null) {
// we've planned out the path, now build up the command
ISpeedGenerator speedGenerator = null;
if (speedCommand is ScalarSpeedCommand) {
// extract the max speed from the planning settings
double maxSpeed = settings.maxSpeed;
if (planningResult.smoothedPath != null) {
SmoothedPath path = planningResult.smoothedPath;
maxSpeed = Math.Min(maxSpeed, PostProcessSpeed(path));
}
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");
}
speedCommandGenerator = new FeedbackSpeedCommandGenerator(speedGenerator);
}
if (cancelled) return;
// build up the command
TrackingCommand trackingCommand = new TrackingCommand(speedCommandGenerator, steeringCommandGenerator, false);
trackingCommand.Label = planningResult.commandLabel ?? commandLabel;
// queue it to execute
Services.TrackingManager.QueueCommand(trackingCommand);
}