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