public static void CalculateMaxPathSpeed(LinePath path, LinePath.PointOnPath startPoint, LinePath.PointOnPath endPoint, ref double maxSpeed)
{
// get the angles
List<Pair<int, double>> angles = path.GetIntersectionAngles(startPoint.Index, endPoint.Index);
foreach (Pair<int, double> angleValue in angles) {
// calculate the desired speed to take the point
double dist = path.DistanceBetween(startPoint, path.GetPointOnPath(angleValue.Left));
// calculate the desired speed at the intersection
double desiredSpeed = 1.5/(angleValue.Right/(Math.PI/2));
// limit the desired speed to 2.5
desiredSpeed = Math.Max(2.5, desiredSpeed);
// calculate the speed we would be allowed to go right now
double desiredMaxSpeed = Math.Sqrt(desiredSpeed*desiredSpeed + 2*target_decel*dist);
// check if the desired max speed is lower
if (desiredMaxSpeed < maxSpeed) {
maxSpeed = desiredMaxSpeed;
}
}
}
public static void CalculateMaxPathSpeed(LinePath path, LinePath.PointOnPath startPoint, LinePath.PointOnPath endPoint, ref double maxSpeed)
{
// get the angles
List <Pair <int, double> > angles = path.GetIntersectionAngles(startPoint.Index, endPoint.Index);
foreach (Pair <int, double> angleValue in angles)
{
// calculate the desired speed to take the point
double dist = path.DistanceBetween(startPoint, path.GetPointOnPath(angleValue.Left));
// calculate the desired speed at the intersection
double desiredSpeed = 1.5 / (angleValue.Right / (Math.PI / 2));
// limit the desired speed to 2.5
desiredSpeed = Math.Max(2.5, desiredSpeed);
// calculate the speed we would be allowed to go right now
double desiredMaxSpeed = Math.Sqrt(desiredSpeed * desiredSpeed + 2 * target_decel * dist);
// check if the desired max speed is lower
if (desiredMaxSpeed < maxSpeed)
{
maxSpeed = desiredMaxSpeed;
}
}
}
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);
}
}
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);
}
}
private ILaneModel GetLaneModel(LocalLaneModel laneModel, LinePath rndfPath, double rndfPathWidth, CarTimestamp rndfPathTimestamp)
{
// check the lane model probability
if (laneModel.Probability < lane_probability_reject_threshold)
{
// we're rejecting this, just return a path lane model
return(new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth));
}
// project the lane model's path into the rndf path's timestamp
RelativeTransform relTransform = Services.RelativePose.GetTransform(localRoadModel.Timestamp, rndfPathTimestamp);
LinePath laneModelPath = laneModel.LanePath.Transform(relTransform);
// iterate through the waypoints in the RNDF path and project onto the lane model
// the first one that is over the threshold, we consider the waypoint before as a potential ending point
LinePath.PointOnPath laneModelDeviationEndPoint = new LinePath.PointOnPath();
// flag indicating if any of the waypoint tests failed because of the devation was too high
bool anyDeviationTooHigh = false;
// number of waypoints accepted
int numWaypointsAccepted = 0;
// get the vehicle's position on the rndf path
LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint;
// get the vehicle's position on the lane model
LinePath.PointOnPath laneModelZeroPoint = laneModelPath.ZeroPoint;
// get the last point we want to consider on the lane model
LinePath.PointOnPath laneModelFarthestPoint = laneModelPath.AdvancePoint(laneModelZeroPoint, lane_model_max_dist);
// start walking forward through the waypoints on the rndf path
// this loop will implicitly exit when we're past the end of the lane model as the waypoints
// will stop being close to the lane model (GetClosestPoint returns the end point if we're past the
// end of the path)
for (int i = rndfZeroPoint.Index + 1; i < rndfPath.Count; i++)
{
// get the waypoint
Coordinates rndfWaypoint = rndfPath[i];
// get the closest point on the lane model
LinePath.PointOnPath laneModelClosestPoint = laneModelPath.GetClosestPoint(rndfWaypoint);
// compute the distance between the two
double deviation = rndfWaypoint.DistanceTo(laneModelClosestPoint.Location);
// if this is above the deviation threshold, leave the loop
if (deviation > lane_deviation_reject_threshold || laneModelClosestPoint > laneModelFarthestPoint)
{
// if we're at the end of the lane model path, we don't want to consider this a rejection
if (laneModelClosestPoint < laneModelFarthestPoint)
{
// mark that at least on deviation was too high
anyDeviationTooHigh = true;
}
break;
}
// increment the number of waypoint accepted
numWaypointsAccepted++;
// update the end point of where we're valid as the local road model was OK up to this point
laneModelDeviationEndPoint = laneModelClosestPoint;
}
// go through and figure out how far out the variance is within tolerance
LinePath.PointOnPath laneModelVarianceEndPoint = new LinePath.PointOnPath();
// walk forward from this point until the end of the lane mode path
for (int i = laneModelZeroPoint.Index + 1; i < laneModelPath.Count; i++)
{
// check if we're within the variance toleration
if (laneModel.LaneYVariance[i] <= y_var_reject_threshold)
{
// we are, update the point on path
laneModelVarianceEndPoint = laneModelPath.GetPointOnPath(i);
}
else
{
// we are out of tolerance, break out of the loop
break;
}
}
// now figure out everything out
// determine waypoint rejection status
WaypointRejectionResult waypointRejectionResult;
if (laneModelDeviationEndPoint.Valid)
{
// if the point is valid, that we had at least one waypoint that was ok
// check if any waypoints were rejected
if (anyDeviationTooHigh)
{
// some waypoint was ok, so we know that at least one waypoint was accepted
waypointRejectionResult = WaypointRejectionResult.SomeWaypointsAccepted;
}
else
{
// no waypoint triggered a rejection, but at least one was good
waypointRejectionResult = WaypointRejectionResult.AllWaypointsAccepted;
}
}
else
{
// the point is not valid, so we either had no waypoints or we had all rejections
if (anyDeviationTooHigh)
{
// the first waypoint was rejected, so all are rejected
waypointRejectionResult = WaypointRejectionResult.AllWaypointsRejected;
}
else
{
// the first waypoint (if any) was past the end of the lane model
waypointRejectionResult = WaypointRejectionResult.NoWaypoints;
}
}
// criteria for determining if this path is valid:
// - if some or all waypoints were accepted, than this is probably a good path
// - if some of the waypoints were accepted, we go no farther than the last waypoint that was accepted
// - if there were no waypoints, this is a potentially dangerous situation since we can't reject
// or confirm the local road model. for now, we'll assume that it is correct but this may need to change
// if we start handling intersections in this framework
// - if all waypoints were rejected, than we don't use the local road model
// - go no farther than the laneModelVarianceEndPoint, which is the last point where the y-variance of
// the lane model was in tolerance
// now build out the lane model
ILaneModel finalLaneModel;
// check if we rejected all waypoints or no lane model points satisified the variance threshold
if (waypointRejectionResult == WaypointRejectionResult.AllWaypointsRejected || !laneModelVarianceEndPoint.Valid)
{
// want to just use the path lane model
finalLaneModel = new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth);
}
else
{
// we'll use the lane model
// need to build up the center line as well as left and right bounds
// to build up the center line, use the lane model as far as we feel comfortable (limited by either variance
// or by rejections) and then use the rndf lane after that.
LinePath centerLine = new LinePath();
// figure out the max distance
// if there were no waypoints, set the laneModelDeviationEndPoint to the end of the lane model
if (waypointRejectionResult == WaypointRejectionResult.NoWaypoints)
{
laneModelDeviationEndPoint = laneModelFarthestPoint;
}
// figure out the closer of the end points
LinePath.PointOnPath laneModelEndPoint = (laneModelDeviationEndPoint < laneModelVarianceEndPoint) ? laneModelDeviationEndPoint : laneModelVarianceEndPoint;
bool endAtWaypoint = laneModelEndPoint == laneModelDeviationEndPoint;
// add the lane model to the center line
centerLine.AddRange(laneModelPath.GetSubpathEnumerator(laneModelZeroPoint, laneModelEndPoint));
// create a list to hold the width expansion values
List <double> widthValue = new List <double>();
// make the width expansion values the width of the path plus the 1-sigma values
for (int i = laneModelZeroPoint.Index; i < laneModelZeroPoint.Index + centerLine.Count; i++)
{
widthValue.Add(laneModel.Width / 2.0 + Math.Sqrt(laneModel.LaneYVariance[i]));
}
// now figure out how to add the rndf path
// get the projection of the lane model end point on the rndf path
LinePath.PointOnPath rndfPathStartPoint = rndfPath.GetClosestPoint(laneModelEndPoint.Location);
// if the closest point is past the end of rndf path, then we don't want to tack anything on
if (rndfPathStartPoint != rndfPath.EndPoint)
{
// get the last segment of the new center line
Coordinates centerLineEndSegmentVec = centerLine.EndSegment.UnitVector;
// get the last point of the new center line
Coordinates laneModelEndLoc = laneModelEndPoint.Location;
// now figure out the distance to the next waypoint
LinePath.PointOnPath rndfNextPoint = new LinePath.PointOnPath();
// figure out if we're ending at a waypoint or not
if (endAtWaypoint)
{
rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index + 1);
// if the distance from the start point to the next point is less than rndf_dist_min, then
// use the waypont after
double dist = rndfPath.DistanceBetween(rndfPathStartPoint, rndfNextPoint);
if (dist < rndf_dist_min)
{
if (rndfPathStartPoint.Index < rndfPath.Count - 2)
{
rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index + 2);
}
else if (rndfPath.DistanceBetween(rndfPathStartPoint, rndfPath.EndPoint) < rndf_dist_min)
{
rndfNextPoint = LinePath.PointOnPath.Invalid;
}
else
{
rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, rndf_dist_min * 2);
}
}
}
else
{
// track the last angle we had
double lastAngle = double.NaN;
// walk down the rndf path until we find a valid point
for (double dist = rndf_dist_min; dist <= rndf_dist_max; dist += rndf_dist_step)
{
// advance from the start point by dist
double distTemp = dist;
rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, ref distTemp);
// if the distTemp is > 0, then we're past the end of the path
if (distTemp > 0)
{
// if we're immediately past the end, we don't want to tack anything on
if (dist == rndf_dist_min)
{
rndfNextPoint = LinePath.PointOnPath.Invalid;
}
break;
}
// check the angle made by the last segment of center line and the segment
// formed between the end point of the center line and this new point
double angle = Math.Acos(centerLineEndSegmentVec.Dot((rndfNextPoint.Location - laneModelEndLoc).Normalize()));
// check if the angle satisfies the threshold or we're increasing the angle
if (Math.Abs(angle) < rndf_angle_threshold || (!double.IsNaN(lastAngle) && angle > lastAngle))
{
// break out of the loop, we're done searching
break;
}
lastAngle = angle;
}
}
// tack on the rndf starting at next point going to the end
if (rndfNextPoint.Valid)
{
LinePath subPath = rndfPath.SubPath(rndfNextPoint, rndfPath.EndPoint);
centerLine.AddRange(subPath);
// insert the lane model end point into the sub path
subPath.Insert(0, laneModelEndLoc);
// get the angles
List <Pair <int, double> > angles = subPath.GetIntersectionAngles(0, subPath.Count - 1);
// add the width of the path inflated by the angles
for (int i = 0; i < angles.Count; i++)
{
// calculate the width expansion factor
// 90 deg, 3x width
// 45 deg, 1.5x width
// 0 deg, 1x width
double widthFactor = Math.Pow(angles[i].Right / (Math.PI / 2.0), 2) * 2 + 1;
// add the width value
widthValue.Add(widthFactor * laneModel.Width / 2);
}
// add the final width
widthValue.Add(laneModel.Width / 2);
}
// set the rndf path start point to be the point we used
rndfPathStartPoint = rndfNextPoint;
}
// for now, calculate the left and right bounds the same way we do for the path lane model
// TODO: figure out if we want to do this more intelligently using knowledge of the lane model uncertainty
LinePath leftBound = centerLine.ShiftLateral(widthValue.ToArray());
// get the next shifts
for (int i = 0; i < widthValue.Count; i++)
{
widthValue[i] = -widthValue[i];
}
LinePath rightBound = centerLine.ShiftLateral(widthValue.ToArray());
// build the final lane model
finalLaneModel = new CombinedLaneModel(centerLine, leftBound, rightBound, laneModel.Width, rndfPathTimestamp);
}
SendLaneModelToUI(finalLaneModel, rndfPathTimestamp);
// output the fit result
return(finalLaneModel);
}
private LaneModelTestResult TestLane(LinePath rndfPath, CarTimestamp rndfPathTimestamp, LocalLaneModel laneModel)
{
// construct the result object to hold stuff
LaneModelTestResult result = new LaneModelTestResult();
// project the lane model's path into the rndf path's timestamp
RelativeTransform relTransform = Services.RelativePose.GetTransform(localRoadModel.Timestamp, rndfPathTimestamp);
LinePath laneModelPath = laneModel.LanePath.Transform(relTransform);
// get the zero point of the lane model path
LinePath.PointOnPath laneZeroPoint = laneModelPath.ZeroPoint;
// get the first waypoint on the RNDF path
LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint;
// get the heading of the rndf path at its zero point and the heading of the lane model at
// the rndf's zero point
LinePath.PointOnPath laneStartPoint = laneModelPath.GetClosestPoint(rndfZeroPoint.Location);
Coordinates laneModelHeading = laneModelPath.GetSegment(laneStartPoint.Index).UnitVector;
Coordinates rndfHeading = rndfPath.GetSegment(rndfZeroPoint.Index).UnitVector;
double angle = Math.Acos(laneModelHeading.Dot(rndfHeading));
// check if the angle is within limits for comparing offset
if (angle < 30 * Math.PI / 180.0)
{
// get the deviation between lane zero point and rndf zero point
result.rndf_deviation = rndfZeroPoint.Location.DistanceTo(laneZeroPoint.Location);
}
// now start check for how many waypoints are accepted
for (int i = rndfZeroPoint.Index + 1; i < rndfPath.Count; i++)
{
// check the distance along the rndf path
double rndfDistAlong = rndfPath.DistanceBetween(rndfZeroPoint, rndfPath.GetPointOnPath(i));
// break out if we're too far along the rndf
if (rndfDistAlong > 50)
{
break;
}
// get the waypoint
Coordinates waypoint = rndfPath[i];
// project on to lane path
LinePath.PointOnPath laneWaypoint = laneModelPath.GetClosestPoint(waypoint);
// check if we're too far along the lane path
double distAlong = laneModelPath.DistanceBetween(laneZeroPoint, laneWaypoint);
if (distAlong > lane_model_max_dist || distAlong < 0)
{
break;
}
// check if the deviation
double dist = waypoint.DistanceTo(laneWaypoint.Location);
// increment appropriate counts
if (dist < lane_deviation_reject_threshold)
{
result.forward_match_count++;
}
else
{
result.forward_rejection_count++;
}
}
// return the result
return(result);
}
private LaneModelTestResult TestLane(LinePath rndfPath, CarTimestamp rndfPathTimestamp, LocalLaneModel laneModel)
{
// construct the result object to hold stuff
LaneModelTestResult result = new LaneModelTestResult();
// project the lane model's path into the rndf path's timestamp
RelativeTransform relTransform = Services.RelativePose.GetTransform(localRoadModel.Timestamp, rndfPathTimestamp);
LinePath laneModelPath = laneModel.LanePath.Transform(relTransform);
// get the zero point of the lane model path
LinePath.PointOnPath laneZeroPoint = laneModelPath.ZeroPoint;
// get the first waypoint on the RNDF path
LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint;
// get the heading of the rndf path at its zero point and the heading of the lane model at
// the rndf's zero point
LinePath.PointOnPath laneStartPoint = laneModelPath.GetClosestPoint(rndfZeroPoint.Location);
Coordinates laneModelHeading = laneModelPath.GetSegment(laneStartPoint.Index).UnitVector;
Coordinates rndfHeading = rndfPath.GetSegment(rndfZeroPoint.Index).UnitVector;
double angle = Math.Acos(laneModelHeading.Dot(rndfHeading));
// check if the angle is within limits for comparing offset
if (angle < 30*Math.PI/180.0) {
// get the deviation between lane zero point and rndf zero point
result.rndf_deviation = rndfZeroPoint.Location.DistanceTo(laneZeroPoint.Location);
}
// now start check for how many waypoints are accepted
for (int i = rndfZeroPoint.Index + 1; i < rndfPath.Count; i++) {
// check the distance along the rndf path
double rndfDistAlong = rndfPath.DistanceBetween(rndfZeroPoint, rndfPath.GetPointOnPath(i));
// break out if we're too far along the rndf
if (rndfDistAlong > 50) {
break;
}
// get the waypoint
Coordinates waypoint = rndfPath[i];
// project on to lane path
LinePath.PointOnPath laneWaypoint = laneModelPath.GetClosestPoint(waypoint);
// check if we're too far along the lane path
double distAlong = laneModelPath.DistanceBetween(laneZeroPoint, laneWaypoint);
if (distAlong > lane_model_max_dist || distAlong < 0) {
break;
}
// check if the deviation
double dist = waypoint.DistanceTo(laneWaypoint.Location);
// increment appropriate counts
if (dist < lane_deviation_reject_threshold) {
result.forward_match_count++;
}
else {
result.forward_rejection_count++;
}
}
// return the result
return result;
}
private ILaneModel GetLaneModel(LocalLaneModel laneModel, LinePath rndfPath, double rndfPathWidth, CarTimestamp rndfPathTimestamp)
{
// check the lane model probability
if (laneModel.Probability < lane_probability_reject_threshold) {
// we're rejecting this, just return a path lane model
return new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth);
}
// project the lane model's path into the rndf path's timestamp
RelativeTransform relTransform = Services.RelativePose.GetTransform(localRoadModel.Timestamp, rndfPathTimestamp);
LinePath laneModelPath = laneModel.LanePath.Transform(relTransform);
// iterate through the waypoints in the RNDF path and project onto the lane model
// the first one that is over the threshold, we consider the waypoint before as a potential ending point
LinePath.PointOnPath laneModelDeviationEndPoint = new LinePath.PointOnPath();
// flag indicating if any of the waypoint tests failed because of the devation was too high
bool anyDeviationTooHigh = false;
// number of waypoints accepted
int numWaypointsAccepted = 0;
// get the vehicle's position on the rndf path
LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint;
// get the vehicle's position on the lane model
LinePath.PointOnPath laneModelZeroPoint = laneModelPath.ZeroPoint;
// get the last point we want to consider on the lane model
LinePath.PointOnPath laneModelFarthestPoint = laneModelPath.AdvancePoint(laneModelZeroPoint, lane_model_max_dist);
// start walking forward through the waypoints on the rndf path
// this loop will implicitly exit when we're past the end of the lane model as the waypoints
// will stop being close to the lane model (GetClosestPoint returns the end point if we're past the
// end of the path)
for (int i = rndfZeroPoint.Index+1; i < rndfPath.Count; i++) {
// get the waypoint
Coordinates rndfWaypoint = rndfPath[i];
// get the closest point on the lane model
LinePath.PointOnPath laneModelClosestPoint = laneModelPath.GetClosestPoint(rndfWaypoint);
// compute the distance between the two
double deviation = rndfWaypoint.DistanceTo(laneModelClosestPoint.Location);
// if this is above the deviation threshold, leave the loop
if (deviation > lane_deviation_reject_threshold || laneModelClosestPoint > laneModelFarthestPoint) {
// if we're at the end of the lane model path, we don't want to consider this a rejection
if (laneModelClosestPoint < laneModelFarthestPoint) {
// mark that at least on deviation was too high
anyDeviationTooHigh = true;
}
break;
}
// increment the number of waypoint accepted
numWaypointsAccepted++;
// update the end point of where we're valid as the local road model was OK up to this point
laneModelDeviationEndPoint = laneModelClosestPoint;
}
// go through and figure out how far out the variance is within tolerance
LinePath.PointOnPath laneModelVarianceEndPoint = new LinePath.PointOnPath();
// walk forward from this point until the end of the lane mode path
for (int i = laneModelZeroPoint.Index+1; i < laneModelPath.Count; i++) {
// check if we're within the variance toleration
if (laneModel.LaneYVariance[i] <= y_var_reject_threshold) {
// we are, update the point on path
laneModelVarianceEndPoint = laneModelPath.GetPointOnPath(i);
}
else {
// we are out of tolerance, break out of the loop
break;
}
}
// now figure out everything out
// determine waypoint rejection status
WaypointRejectionResult waypointRejectionResult;
if (laneModelDeviationEndPoint.Valid) {
// if the point is valid, that we had at least one waypoint that was ok
// check if any waypoints were rejected
if (anyDeviationTooHigh) {
// some waypoint was ok, so we know that at least one waypoint was accepted
waypointRejectionResult = WaypointRejectionResult.SomeWaypointsAccepted;
}
else {
// no waypoint triggered a rejection, but at least one was good
waypointRejectionResult = WaypointRejectionResult.AllWaypointsAccepted;
}
}
else {
// the point is not valid, so we either had no waypoints or we had all rejections
if (anyDeviationTooHigh) {
// the first waypoint was rejected, so all are rejected
waypointRejectionResult = WaypointRejectionResult.AllWaypointsRejected;
}
else {
// the first waypoint (if any) was past the end of the lane model
waypointRejectionResult = WaypointRejectionResult.NoWaypoints;
}
}
// criteria for determining if this path is valid:
// - if some or all waypoints were accepted, than this is probably a good path
// - if some of the waypoints were accepted, we go no farther than the last waypoint that was accepted
// - if there were no waypoints, this is a potentially dangerous situation since we can't reject
// or confirm the local road model. for now, we'll assume that it is correct but this may need to change
// if we start handling intersections in this framework
// - if all waypoints were rejected, than we don't use the local road model
// - go no farther than the laneModelVarianceEndPoint, which is the last point where the y-variance of
// the lane model was in tolerance
// now build out the lane model
ILaneModel finalLaneModel;
// check if we rejected all waypoints or no lane model points satisified the variance threshold
if (waypointRejectionResult == WaypointRejectionResult.AllWaypointsRejected || !laneModelVarianceEndPoint.Valid) {
// want to just use the path lane model
finalLaneModel = new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth);
}
else {
// we'll use the lane model
// need to build up the center line as well as left and right bounds
// to build up the center line, use the lane model as far as we feel comfortable (limited by either variance
// or by rejections) and then use the rndf lane after that.
LinePath centerLine = new LinePath();
// figure out the max distance
// if there were no waypoints, set the laneModelDeviationEndPoint to the end of the lane model
if (waypointRejectionResult == WaypointRejectionResult.NoWaypoints) {
laneModelDeviationEndPoint = laneModelFarthestPoint;
}
// figure out the closer of the end points
LinePath.PointOnPath laneModelEndPoint = (laneModelDeviationEndPoint < laneModelVarianceEndPoint) ? laneModelDeviationEndPoint : laneModelVarianceEndPoint;
bool endAtWaypoint = laneModelEndPoint == laneModelDeviationEndPoint;
// add the lane model to the center line
centerLine.AddRange(laneModelPath.GetSubpathEnumerator(laneModelZeroPoint, laneModelEndPoint));
// create a list to hold the width expansion values
List<double> widthValue = new List<double>();
// make the width expansion values the width of the path plus the 1-sigma values
for (int i = laneModelZeroPoint.Index; i < laneModelZeroPoint.Index+centerLine.Count; i++) {
widthValue.Add(laneModel.Width/2.0 + Math.Sqrt(laneModel.LaneYVariance[i]));
}
// now figure out how to add the rndf path
// get the projection of the lane model end point on the rndf path
LinePath.PointOnPath rndfPathStartPoint = rndfPath.GetClosestPoint(laneModelEndPoint.Location);
// if the closest point is past the end of rndf path, then we don't want to tack anything on
if (rndfPathStartPoint != rndfPath.EndPoint) {
// get the last segment of the new center line
Coordinates centerLineEndSegmentVec = centerLine.EndSegment.UnitVector;
// get the last point of the new center line
Coordinates laneModelEndLoc = laneModelEndPoint.Location;
// now figure out the distance to the next waypoint
LinePath.PointOnPath rndfNextPoint = new LinePath.PointOnPath();
// figure out if we're ending at a waypoint or not
if (endAtWaypoint) {
rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index+1);
// if the distance from the start point to the next point is less than rndf_dist_min, then
// use the waypont after
double dist = rndfPath.DistanceBetween(rndfPathStartPoint, rndfNextPoint);
if (dist < rndf_dist_min) {
if (rndfPathStartPoint.Index < rndfPath.Count-2) {
rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index + 2);
}
else if (rndfPath.DistanceBetween(rndfPathStartPoint, rndfPath.EndPoint) < rndf_dist_min) {
rndfNextPoint = LinePath.PointOnPath.Invalid;
}
else {
rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, rndf_dist_min*2);
}
}
}
else {
// track the last angle we had
double lastAngle = double.NaN;
// walk down the rndf path until we find a valid point
for (double dist = rndf_dist_min; dist <= rndf_dist_max; dist += rndf_dist_step) {
// advance from the start point by dist
double distTemp = dist;
rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, ref distTemp);
// if the distTemp is > 0, then we're past the end of the path
if (distTemp > 0) {
// if we're immediately past the end, we don't want to tack anything on
if (dist == rndf_dist_min) {
rndfNextPoint = LinePath.PointOnPath.Invalid;
}
break;
}
// check the angle made by the last segment of center line and the segment
// formed between the end point of the center line and this new point
double angle = Math.Acos(centerLineEndSegmentVec.Dot((rndfNextPoint.Location-laneModelEndLoc).Normalize()));
// check if the angle satisfies the threshold or we're increasing the angle
if (Math.Abs(angle) < rndf_angle_threshold || (!double.IsNaN(lastAngle) && angle > lastAngle)) {
// break out of the loop, we're done searching
break;
}
lastAngle = angle;
}
}
// tack on the rndf starting at next point going to the end
if (rndfNextPoint.Valid) {
LinePath subPath = rndfPath.SubPath(rndfNextPoint, rndfPath.EndPoint);
centerLine.AddRange(subPath);
// insert the lane model end point into the sub path
subPath.Insert(0, laneModelEndLoc);
// get the angles
List<Pair<int, double>> angles = subPath.GetIntersectionAngles(0, subPath.Count-1);
// add the width of the path inflated by the angles
for (int i = 0; i < angles.Count; i++) {
// calculate the width expansion factor
// 90 deg, 3x width
// 45 deg, 1.5x width
// 0 deg, 1x width
double widthFactor = Math.Pow(angles[i].Right/(Math.PI/2.0), 2)*2 + 1;
// add the width value
widthValue.Add(widthFactor*laneModel.Width/2);
}
// add the final width
widthValue.Add(laneModel.Width/2);
}
// set the rndf path start point to be the point we used
rndfPathStartPoint = rndfNextPoint;
}
// for now, calculate the left and right bounds the same way we do for the path lane model
// TODO: figure out if we want to do this more intelligently using knowledge of the lane model uncertainty
LinePath leftBound = centerLine.ShiftLateral(widthValue.ToArray());
// get the next shifts
for (int i = 0; i < widthValue.Count; i++) { widthValue[i] = -widthValue[i]; }
LinePath rightBound = centerLine.ShiftLateral(widthValue.ToArray());
// build the final lane model
finalLaneModel = new CombinedLaneModel(centerLine, leftBound, rightBound, laneModel.Width, rndfPathTimestamp);
}
SendLaneModelToUI(finalLaneModel, rndfPathTimestamp);
// output the fit result
return finalLaneModel;
}
protected virtual double GetMaxSpeed(LinePath rndfPath, LinePath.PointOnPath startingPoint)
{
double maxSpeed;
if (speedCommand is ScalarSpeedCommand) {
maxSpeed = Math.Min(((ScalarSpeedCommand)speedCommand).Speed, 15);
// calculate the swerving max speed
double deltaSteeringSigma = Services.TrackingManager.DeltaSteeringStdDev*180/Math.PI;
if (deltaSteeringSigma > 3) {
double swerveMax = Math.Max(18 - 0.2*deltaSteeringSigma, 2.5);
maxSpeed = Math.Min(maxSpeed, swerveMax);
}
approachSpeed = null;
}
else {
// assume that it's a stop at dist or stop at line
// either way, we do the same thing
if (approachSpeed == null || (double)approachSpeed < 1) {
approachSpeed = Math.Max(vs.speed, 1);
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "approach speed set to {0}", approachSpeed.Value);
}
maxSpeed = approachSpeed.Value;
}
if (rndfPath != null) {
// get the angles
List<Pair<int, double>> angles = rndfPath.GetIntersectionAngles(startingPoint.Index, rndfPath.Count-1);
foreach (Pair<int, double> angleValue in angles) {
// calculate the desired speed to take the point
double dist = Math.Max(rndfPath.DistanceBetween(startingPoint, rndfPath.GetPointOnPath(angleValue.Left))-TahoeParams.FL, 0);
// calculate the desired speed at the intersection
double desiredSpeed = 1.5/(angleValue.Right/(Math.PI/2));
// limit the desired speed to 2.5
desiredSpeed = Math.Max(2.5, desiredSpeed);
// calculate the speed we would be allowed to go right now
double desiredMaxSpeed = Math.Sqrt(desiredSpeed*desiredSpeed + 2*target_decel*dist);
// check if the desired max speed is lower
if (desiredMaxSpeed < maxSpeed) {
maxSpeed = desiredMaxSpeed;
}
}
}
return maxSpeed;
}
private LinePath ReplaceFirstPoint(LinePath path, LinePath.PointOnPath maxAdvancePoint, Coordinates pt)
{
LinePath ret;
if (!disablePathAngleCheck) {
// start walking down the path until the angle is cool
double angle_threshold = pathAngleMax;
LinePath.PointOnPath newPoint = new LinePath.PointOnPath();
for (double dist = 0; dist < pathAngleCheckMax; dist += 1) {
// get the point advanced from the 2nd point on the base path by dist
double distTemp = dist;
newPoint = path.AdvancePoint(path.GetPointOnPath(1), ref distTemp);
// check if we're past the end
if (distTemp > 0) {
break;
}
else if (maxAdvancePoint.Valid && newPoint >= maxAdvancePoint) {
newPoint = maxAdvancePoint;
break;
}
// check if the angle is coolness or not
double angle = Math.Acos((newPoint.Location-pt).Normalize().Dot(path.GetSegment(newPoint.Index).UnitVector));
if (angle < angle_threshold) {
break;
}
}
// create a new version of the base path with the stuff section removed
ret = path.RemoveBetween(path.StartPoint, newPoint);
ret[0] = pt;
}
else {
ret = path.Clone();
ret[0] = pt;
}
return ret;
}