/// <summary>
/// Reason about changing lanes
/// </summary>
/// <param name="previousChangeLanePath">previous change lane path</param>
/// <param name="initialLanePath">lane path that vehicle is changing from</param>
/// <param name="targetLanePath"> lane path that vehicle is changing to</param>
/// <param name="targetType"> type for target lane, left or right</param>
/// <param name="observedObstacles">static obstacles</param>
/// <param name="observedVehicles">observed vehicles</param>
/// <param name="lowerBound"> lower bound point on initial lane (similar to obstacle on target lane)</param>
/// <param name="upperBound"> upper bound point on initial lane (similar to obstacle on initial lane)</param>
/// <param name="position"> vehicle absolute position in m</param>
/// <param name="heading"> vehicle heading as a vector</param>
/// <param name="speed"> vehicle speed in m/s</param>
/// <param name="aboutPath"> type of path being returned</param>
/// <param name="currentChangeLanePath">change lane path</param>
public void LaneChangePlanAdvance(Path previousChangeLanePath,
Path initialLanePath, Path targetLanePath,
TargetLaneChangeType targetType,
ObservedObstacles observedObstacles,
ObservedVehicle[] observedVehicles,
PointOnPath initialLaneLowerBound, PointOnPath initialLaneUpperBound,
Coordinates position, Coordinates heading, double speed,
out AboutPath aboutPath, out Path currentChangeLanePath)
{
// check if target lane is to the left or right
if (targetType == TargetLaneChangeType.Left) {
// set up vehicle and lane information
InitialiseInformation(position, heading, speed, null, targetLanePath, initialLanePath);
// manage static and dynamic dynamic obstacles
InitialiseObstacles(null, targetLanePath, initialLanePath, observedObstacles, observedVehicles);
}
else {
// set up vehicle and lane information
InitialiseInformation(position, heading, speed, initialLanePath, targetLanePath, null);
// manage static and dynamic dynamic obstacles
InitialiseObstacles(initialLanePath, targetLanePath, null, observedObstacles, observedVehicles);
}
// determine risk of previous spline path, if provided
double pathRisk, pathRiskDist, pathSepDist;
if (previousChangeLanePath != null) {
// check risk of previous spline path
CheckPathRisk(previousChangeLanePath, out pathRisk, out pathRiskDist, out pathSepDist);
if (pathRisk == 0) {
// no risk was found, return previous spline path
currentChangeLanePath = previousChangeLanePath;
aboutPath = AboutPath.Normal;
return;
}
}
PointOnPath targetLaneLowerBound = targetLanePath.GetClosest(initialLaneLowerBound.pt);
PointOnPath targetLaneUpperBound = targetLanePath.GetClosest(initialLaneUpperBound.pt);
double targetLaneLowerBoundDist = Math.Round(targetLanePath.DistanceBetween(currentLanePosition, targetLaneLowerBound),1);
double targetLaneUpperBoundDist = Math.Round(targetLanePath.DistanceBetween(currentLanePosition, targetLaneUpperBound),1);
// generate obstacles for lower and upper bound points
Coordinates lowerBoundObstacle = targetLaneLowerBound.pt;
Coordinates upperBoundObstacle = initialLaneUpperBound.pt;
if (targetType == TargetLaneChangeType.Left) {
lowerBoundObstacle += targetLaneLowerBound.segment.Tangent(targetLaneLowerBound).RotateM90().Normalize(0.5 * currentLaneWidth - 1.0);
upperBoundObstacle += initialLaneUpperBound.segment.Tangent(initialLaneUpperBound).Rotate90().Normalize(0.5 * rightLaneWidth - 1.0);
}
else {
lowerBoundObstacle += targetLaneLowerBound.segment.Tangent(targetLaneLowerBound).Rotate90().Normalize(0.5 * currentLaneWidth - 1.0);
upperBoundObstacle += initialLaneUpperBound.segment.Tangent(initialLaneUpperBound).RotateM90().Normalize(0.5 * leftLaneWidth - 1.0);
}
staticObstaclesFake.Add(lowerBoundObstacle);
staticObstaclesFake.Add(upperBoundObstacle);
// path projection distance
double projectionDist = Math.Max(targetLaneLowerBoundDist, TahoeParams.VL + TahoeParams.FL);
double origProjectionDist = projectionDist;
do {
// lookahead point
double lookaheadDist = projectionDist;
PointOnPath lookaheadPt = targetLanePath.AdvancePoint(currentLanePosition, ref lookaheadDist);
// extend point if at end of path
Coordinates offsetVec = new Coordinates(0, 0);
if (lookaheadDist > 0.5)
offsetVec = lookaheadPt.segment.Tangent(lookaheadPt).Normalize(lookaheadDist);
// prepare ctrl points for spline path
Coordinates startPoint = vehiclePosition;
Coordinates endPoint = lookaheadPt.pt + offsetVec;
Coordinates startVec = new Coordinates(1, 0).Rotate(vehicleHeading).Normalize(Math.Max(vehicleSpeed, 2.0));
Coordinates endVec = lookaheadPt.segment.Tangent(lookaheadPt).Normalize(Math.Max(vehicleSpeed, 2.0));
// generate spline path
currentChangeLanePath = GenerateBezierPath(startPoint, endPoint, startVec, endVec);
// determine risk of spline path
CheckPathRisk(currentChangeLanePath, out pathRisk, out pathRiskDist, out pathSepDist);
// project further if current spline path has risk
if (pathRisk != 0) {
if (projectionDist == targetLaneUpperBoundDist + TahoeParams.RL)
break;
projectionDist = Math.Min(projectionDist + TahoeParams.VL / 2, targetLaneUpperBoundDist + TahoeParams.RL);
}
} while (pathRisk != 0 && projectionDist <= targetLaneUpperBoundDist + TahoeParams.RL);
// check if path without risk was found
if (pathRisk == 0)
aboutPath = AboutPath.Normal;
else
aboutPath = AboutPath.Null;
}
/// <summary>
/// Reason about changing lanes
/// </summary>
/// <param name="previousChangeLanePath">previous change lane path</param>
/// <param name="initialLanePath">lane path that vehicle is changing from</param>
/// <param name="targetLanePath"> lane path that vehicle is changing to</param>
/// <param name="targetType">type for target lane, left or right</param>
/// <param name="initialLaneVehicles">observed vehicles on initial lane</param>
/// <param name="initialLaneLowerBound">lower bound point on initial lane (similar to obstacle on target lane)</param>
/// <param name="initialLaneUpperBound">upper bound point on initial lane (similar to obstacle on initial lane)</param>
/// <param name="vehicleState">vehicle state</param>
public Path LaneChangeObstacleReasoning(Path previousChangeLanePath,
Path initialLanePath, Path targetLanePath,
TargetLaneChangeType targetType,
ObservedVehicle[] initialLaneVehicles,
PointOnPath initialLaneLowerBound,
PointOnPath initialLaneUpperBound,
VehicleState vehicleState)
{
// check if target lane is to the left or right
if (targetType == TargetLaneChangeType.Left) {
// set up vehicle and lane information
InitialiseInformation(vehicleState.xyPosition, vehicleState.heading, vehicleState.speed,
null, targetLanePath, initialLanePath);
}
else {
// set up vehicle and lane information
InitialiseInformation(vehicleState.xyPosition, vehicleState.heading, vehicleState.speed,
initialLanePath, targetLanePath, null);
}
// set up static obstacles (none for now)
staticObstaclesIn.Clear();
staticObstaclesOut.Clear();
staticObstaclesFake.Clear();
// set up dynamic obstacles
dynamicObstacles.Clear();
dynamicObstaclesPaths.Clear();
SetDynamicObstacles(initialLanePath, initialLaneVehicles);
// determine risk of previous spline path, if provided
double pathRisk, pathRiskDist, pathSepDist;
if (previousChangeLanePath != null) {
// check risk of previous spline path
CheckPathRisk(previousChangeLanePath, out pathRisk, out pathRiskDist, out pathSepDist);
// if no risk was found, return previous spline path
if (pathRisk == 0)
return previousChangeLanePath;
}
// set up number of paths based on lane width
double spacing = 0.25;
int numPaths = (int)Math.Round(currentLaneWidth / spacing);
if ((int)Math.IEEERemainder((double)numPaths, 2.0) == 0)
numPaths -= 1;
// increase number of drift paths
int midPathIndex;
numPaths += 12;
midPathIndex = (numPaths - 1) / 2;
double[] pathsRisk, pathsRiskDist, pathsSepDist, pathsCost;
Path[] paths = new Path[numPaths];
int selectedPathIndex;
PointOnPath targetLaneLowerBound = targetLanePath.GetClosest(initialLaneLowerBound.pt);
PointOnPath targetLaneUpperBound = targetLanePath.GetClosest(initialLaneUpperBound.pt);
double targetLaneLowerBoundDist = Math.Round(targetLanePath.DistanceBetween(currentLanePosition, targetLaneLowerBound), 1);
double targetLaneUpperBoundDist = Math.Round(targetLanePath.DistanceBetween(currentLanePosition, targetLaneUpperBound), 1);
// generate obstacles for lower and upper bound points
Coordinates lowerBoundObstacle = targetLaneLowerBound.pt;
Coordinates upperBoundObstacle = initialLaneUpperBound.pt;
if (targetType == TargetLaneChangeType.Left) {
lowerBoundObstacle += targetLaneLowerBound.segment.Tangent(targetLaneLowerBound).RotateM90().Normalize(0.5 * currentLaneWidth - 1.0);
upperBoundObstacle += initialLaneUpperBound.segment.Tangent(initialLaneUpperBound).Rotate90().Normalize(0.5 * rightLaneWidth - 1.0);
}
else {
lowerBoundObstacle += targetLaneLowerBound.segment.Tangent(targetLaneLowerBound).Rotate90().Normalize(0.5 * currentLaneWidth - 1.0);
upperBoundObstacle += initialLaneUpperBound.segment.Tangent(initialLaneUpperBound).RotateM90().Normalize(0.5 * leftLaneWidth - 1.0);
}
staticObstaclesFake.Add(lowerBoundObstacle);
staticObstaclesFake.Add(upperBoundObstacle);
// path projection distance
double projectionDist = Math.Max(targetLaneLowerBoundDist, TahoeParams.VL + TahoeParams.FL);
double origProjectionDist = projectionDist;
Path currentChangeLanePath = new Path();
do {
// lookahead point
double lookaheadDist = projectionDist;
PointOnPath lookaheadPt = targetLanePath.AdvancePoint(currentLanePosition, ref lookaheadDist);
// extend point if at end of path
Coordinates offsetVec = new Coordinates(0, 0);
if (lookaheadDist > 0.5)
offsetVec = lookaheadPt.segment.Tangent(lookaheadPt).Normalize(lookaheadDist);
// prepare ctrl points for first part of spline path
Coordinates startPoint = vehiclePosition;
Coordinates midPoint = lookaheadPt.pt + offsetVec;
Coordinates startVec = new Coordinates(1, 0).Rotate(vehicleHeading).Normalize(Math.Max(vehicleSpeed, 2.0));
Coordinates midVec = lookaheadPt.segment.Tangent(lookaheadPt).Normalize(Math.Max(vehicleSpeed, 2.0));
// lookahead point (for end point)
lookaheadDist = projectionDist + 10;
lookaheadPt = targetLanePath.AdvancePoint(currentLanePosition, ref lookaheadDist);
// extend point if at end of path (for end point)
offsetVec = new Coordinates(0, 0);
if (lookaheadDist > 0.5)
offsetVec = lookaheadPt.segment.Tangent(lookaheadPt).Normalize(lookaheadDist);
// prepare ctrl points for second part of spline path
Coordinates endPoint = lookaheadPt.pt + offsetVec;
Coordinates endVec = lookaheadPt.segment.Tangent(lookaheadPt).Normalize(Math.Max(vehicleSpeed, 2.0));
/////////////////////////////////
Coordinates shiftedMidPoint, shiftedEndPoint;
Coordinates shiftMidVec = midVec.Rotate90();
Coordinates shiftEndVec = endVec.Rotate90();
// generate multiple spline paths
for (int i = 0; i < numPaths; i++) {
shiftedMidPoint = midPoint - shiftMidVec.Normalize((i - midPathIndex) * spacing);
shiftedEndPoint = endPoint - shiftEndVec.Normalize((i - midPathIndex) * spacing);
// generate spline path
paths[i] = GenerateBezierPath(startPoint, shiftedMidPoint, startVec, midVec);
// generate extension to spline path
Path extPath = GenerateBezierPath(shiftedMidPoint, shiftedEndPoint, midVec, endVec);
// add extension to path
paths[i].Add((BezierPathSegment)extPath[0]);
}
// evaluate paths and select safest path
selectedPathIndex = EvaluatePaths(paths, midPathIndex, out pathsRisk, out pathsRiskDist, out pathsSepDist, out pathsCost);
// project further if current spline path has risk
if (pathsRisk[selectedPathIndex] != 0) {
if (projectionDist == targetLaneUpperBoundDist + TahoeParams.RL)
break;
projectionDist = Math.Min(projectionDist + TahoeParams.VL / 2, targetLaneUpperBoundDist + TahoeParams.RL);
}
} while (pathsRisk[selectedPathIndex] != 0 && projectionDist <= targetLaneUpperBoundDist + TahoeParams.RL);
// check if path without risk was found
if (pathsRisk[selectedPathIndex] == 0)
return paths[selectedPathIndex];
else
return null;
}
