/// <summary>
/// Get the closest points on 2 paths
/// </summary>
/// <param name="path1"></param>
/// <param name="path2"></param>
/// <param name="p1"></param>
/// <param name="p2"></param>
public static void GetClosestPoints(Path path1, Path path2, out PointOnPath p1, out PointOnPath p2, out double distance)
{
PointOnPath closest1 = path1.StartPoint;
PointOnPath closest2 = path2.StartPoint;
double dist = Double.MaxValue;
PointOnPath current = path1.StartPoint;
double increment = 1;
double tmpIncrement = 0;
while (tmpIncrement == 0)
{
PointOnPath test = path2.GetClosest(current.pt);
double tmpDist = test.pt.DistanceTo(current.pt);
if (tmpDist < dist)
{
closest1 = current;
closest2 = test;
dist = tmpDist;
}
tmpIncrement = increment;
current = path1.AdvancePoint(current, ref tmpIncrement);
}
p1 = closest1;
p2 = closest2;
distance = dist;
}
/// <summary>
/// Produces an obstacle reasoning path for lane type situations (Version 1)
/// </summary>
/// <param name="leftLanePath">The path representing the left lane</param>
/// <param name="leftLaneIsOncoming">Whether the left lane path is oncoming or not</param>
/// <param name="leftLaneVehicles">The vehicles referenced to the left lane</param>
/// <param name="currentLaneDefaultPath">The default path for the current lane</param>
/// <param name="rightLanePath">The path of the right lane, always going in our same direction</param>
/// <param name="rightLaneVehicles">The vehicles referenced to the right lane</param>
/// <param name="vehicleState">Our current vehicle state</param>
/// <returns>A modified lane path that avoids the vehicles in the adjacent lanes while staying in the current lane</returns>
public Path LaneObstacleReasoningVer1(Path leftLanePath, bool leftLaneIsOncoming,
ObservedVehicle[] leftLaneVehicles,
Path currentLanePath,
Path rightLanePath,
ObservedVehicle[] rightLaneVehicles,
VehicleState vehicleState)
{
// set up vehicle and lane information
InitialiseInformation(vehicleState.xyPosition, vehicleState.heading, vehicleState.speed,
leftLanePath, currentLanePath, rightLanePath);
// set up static obstacles (none for now)
staticObstaclesIn.Clear();
staticObstaclesOut.Clear();
staticObstaclesFake.Clear();
// set up dynamic obstacles
dynamicObstacles.Clear();
dynamicObstaclesPaths.Clear();
SetDynamicObstacles(leftLanePath, leftLaneVehicles);
SetDynamicObstacles(rightLanePath, rightLaneVehicles);
double projectionDist = Math.Max(vehicleSpeed * 3, 10) + TahoeParams.FL;
double origProjectionDist = projectionDist;
// 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;
if (leftLaneIsOncoming == true) {
midPathIndex = (numPaths - 1) / 2;
numPaths += 6;
}
else {
numPaths += 12;
midPathIndex = (numPaths - 1) / 2;
}
double[] pathsRisk, pathsRiskDist, pathsSepDist, pathsCost;
Path[] paths = new Path[numPaths];
int selectedPathIndex;
do {
// lookahead point
double lookaheadDist = projectionDist;
PointOnPath lookaheadPt = currentLanePath.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));
Coordinates shiftVec = endVec.Rotate90();
// generate multiple spline paths
for (int i = 0; i < numPaths; i++) {
// generate spline path
paths[i] = GenerateBezierPath(startPoint, endPoint - shiftVec.Normalize((i - midPathIndex) * spacing), startVec, endVec);
}
// evaluate paths and select safest path
selectedPathIndex = EvaluatePaths(paths, midPathIndex, out pathsRisk, out pathsRiskDist, out pathsSepDist, out pathsCost);
if (pathsRisk[selectedPathIndex] != 0)
projectionDist = Math.Max(pathsRiskDist[selectedPathIndex] - 1, 0);
} while (pathsRisk[selectedPathIndex] != 0 && projectionDist > 7.5);
// return back safest path
return paths[selectedPathIndex];
}
/// <summary>
/// Produces an obstacle reasoning path for lane type situations (Version 2b - Latest)
/// </summary>
/// <param name="leftLanePath">The path representing the left lane</param>
/// <param name="leftLaneIsOncoming">Whether the left lane path is oncoming or not</param>
/// <param name="leftLaneVehicles">The vehicles referenced to the left lane</param>
/// <param name="currentLaneDefaultPath">The default path for the current lane</param>
/// <param name="rightLanePath">The path of the right lane, always going in our same direction</param>
/// <param name="rightLaneVehicles">The vehicles referenced to the right lane</param>
/// <param name="vehicleState">Our current vehicle state</param>
/// <returns>A modified lane path that avoids the vehicles in the adjacent lanes while staying in the current lane</returns>
public Path LaneObstacleReasoning(Path leftLanePath, bool leftLaneIsOncoming,
ObservedVehicle[] leftLaneVehicles,
Path currentLanePath,
Path rightLanePath,
ObservedVehicle[] rightLaneVehicles,
VehicleState vehicleState)
{
// set up vehicle and lane information
InitialiseInformation(vehicleState.xyPosition, vehicleState.heading, vehicleState.speed,
leftLanePath, currentLanePath, rightLanePath);
// set up static obstacles (none for now)
staticObstaclesIn.Clear();
staticObstaclesOut.Clear();
staticObstaclesFake.Clear();
// set up dynamic obstacles
dynamicObstacles.Clear();
dynamicObstaclesPaths.Clear();
SetDynamicObstacles(leftLanePath, leftLaneVehicles);
SetDynamicObstacles(rightLanePath, rightLaneVehicles);
double projectionDist = Math.Max(vehicleSpeed * 5, 10) + TahoeParams.FL;
double origProjectionDist = projectionDist;
// 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;
if (leftLaneIsOncoming == true) {
midPathIndex = (numPaths - 1) / 2;
numPaths += 6;
}
else {
numPaths += 12;
midPathIndex = (numPaths - 1) / 2;
}
double[] pathsRisk, pathsRiskDist, pathsSepDist, pathsCost;
Path[] paths = new Path[numPaths];
int selectedPathIndex;
do {
// lookahead point
double lookaheadDist = projectionDist;
PointOnPath lookaheadPt = currentLanePath.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 mid points of path
int midPointsTotal = (int)Math.Round(projectionDist / 5.0) - 1;
double midPointStepDist = projectionDist / (midPointsTotal + 1);
Coordinates[] midPoints = new Coordinates[midPointsTotal];
Coordinates[] midVecs = new Coordinates[midPointsTotal];
Coordinates[] midShiftVecs = new Coordinates[midPointsTotal];
for (int i = 0; i < midPointsTotal; i++) {
// lookahead point
lookaheadDist = projectionDist * (i + 1) / (midPointsTotal + 1);
lookaheadPt = currentLanePath.AdvancePoint(currentLanePosition, ref lookaheadDist);
// extend point if at end of path
offsetVec = new Coordinates(0, 0);
if (lookaheadDist > 0.5)
offsetVec = lookaheadPt.segment.Tangent(lookaheadPt).Normalize(lookaheadDist);
// prepare ctrl points for spline path
midPoints[i] = lookaheadPt.pt + offsetVec;
midVecs[i] = lookaheadPt.segment.Tangent(lookaheadPt).Normalize(Math.Max(vehicleSpeed, 2.0));
midShiftVecs[i] = midVecs[i].Rotate90();
}
Coordinates endShiftVec = endVec.Rotate90();
// generate multiple spline paths
for (int i = 0; i < numPaths; i++) {
// vehicle vector with respect to segment closest point
Coordinates carVec = vehiclePosition - currentLanePosition.pt;
// segment tangent vector
Coordinates pathVec = currentLanePosition.segment.Tangent(currentLanePosition);
// compute offtrack error
double offtrackError = Math.Sign(carVec.Cross(pathVec)) * currentLanePosition.pt.DistanceTo(vehiclePosition);
// path points
Coordinates[] pathPoints = new Coordinates[midPointsTotal + 2];
pathPoints[0] = startPoint;
pathPoints[midPointsTotal + 1] = endPoint - endShiftVec.Normalize((i - midPathIndex) * spacing);
for (int j = 0; j < midPointsTotal; j++) {
double control = 0.0;
if (j == 0)
control = 0.35;
else if (j == midPointsTotal - 1)
control = -0.35;
pathPoints[j+1] = midPoints[j] -
midShiftVecs[j].Normalize((i - midPathIndex) * spacing * (j + 1 - control) / (midPointsTotal + 1) +
offtrackError * (midPointsTotal - j + control) / (midPointsTotal + 1));
}
// generate spline path with points
paths[i] = new Path();
CubicBezier[] beziers = SmoothingSpline.BuildC2Spline(pathPoints, startVec.Normalize(0.5 * midPointStepDist),
endVec.Normalize(0.5 * midPointStepDist), 0.5);
for (int j = 0; j < beziers.Length; j++) {
paths[i].Add(new BezierPathSegment(beziers[j], (double?)null, false));
}
}
// evaluate paths and select safest path
selectedPathIndex = EvaluatePaths(paths, midPathIndex, out pathsRisk, out pathsRiskDist, out pathsSepDist, out pathsCost);
if (pathsRisk[selectedPathIndex] != 0)
projectionDist = Math.Max(pathsRiskDist[selectedPathIndex] - 1, 0);
} while (pathsRisk[selectedPathIndex] != 0 && projectionDist > 7.5);
// return back safest path
//int index = DateTime.Now.Second;
//selectedPathIndex = index - (paths.Length - 1) * (int)Math.Floor((double)index / (paths.Length - 1));
return paths[selectedPathIndex];
}
/// <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 (simple version)
/// </summary>
public void LaneChangePlan(Path changeLanesPath, Path initialLane, Path targetLane,
ObservedObstacles observedObstacles,
ObservedVehicle[] initialLaneObservedVehicles,
ObservedVehicle[] targetLaneObservedVehicles,
PointOnPath lowerBound, PointOnPath upperBound,
Coordinates position, Coordinates heading, double speed,
out AboutPath aboutPath, out Path laneChangePath)
{
// set up vehicle states
vehiclePosition = position;
vehicleSpeed = speed;
vehicleHeading = heading.ArcTan;
currentLanePosition = targetLane.GetClosest(vehiclePosition);
leftLanePosition = initialLane != null ? initialLane.GetClosest(vehiclePosition) : new PointOnPath();
rightLanePosition = initialLane != null ? initialLane.GetClosest(vehiclePosition) : new PointOnPath();
//// set up lane information
leftLaneWidth = initialLane != null ? leftLanePosition.pt.DistanceTo(currentLanePosition.pt) : double.NaN;
rightLaneWidth = initialLane != null ? rightLanePosition.pt.DistanceTo(currentLanePosition.pt) : double.NaN;
if (double.IsNaN(leftLaneWidth))
if (double.IsNaN(rightLaneWidth))
currentLaneWidth = 3;
else
currentLaneWidth = rightLaneWidth;
else
currentLaneWidth = leftLaneWidth;
//// manage static and dynamic dynamic obstacles
//ManageObstacles(targetLane, observedObstacles,
// initialLaneObservedVehicles, targetLaneObservedVehicles);
double projectionDist = 15;
double origProjDist = projectionDist;
double pathRisk, pathRiskDist, pathSepDist;
// lookahead point
double lookaheadDist = projectionDist;
PointOnPath lookaheadPt = targetLane.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);
PointOnPath targetUpperBound = targetLane.GetClosest(upperBound.pt);
// prepare ctrl points for spline path
Coordinates startPoint = vehiclePosition;
//Coordinates rVec = lookaheadPt.segment.Tangent(lookaheadPt).Rotate90().Normalize(endOffset);
Coordinates endPoint = targetUpperBound.pt; // lookaheadPt.pt + offsetVec + rVec;
Coordinates startVec = new Coordinates(1, 0).Rotate(vehicleHeading).Normalize(Math.Max(vehicleSpeed, 2.0));
Coordinates endVec = targetLane.GetClosest(targetUpperBound.pt).segment.Tangent(targetUpperBound).Normalize(Math.Max(vehicleSpeed, 2.0));
//Coordinates endVec = lookaheadPt.segment.Tangent(lookaheadPt).Normalize(Math.Max(vehicleSpeed, 2.0));
// generate spline path
laneChangePath = GenerateBezierPath(startPoint, endPoint, startVec, endVec);
// determine risk of spline path
CheckPathRisk(laneChangePath, out pathRisk, out pathRiskDist, out pathSepDist);
if (pathRisk == 0)
aboutPath = AboutPath.Normal;
else
aboutPath = AboutPath.Stop;
}
/// <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;
}
/// <summary>
/// Reason about travelling the lane ahead
/// </summary>
/// <param name="currentLanePath">lane path that vehicle is following</param>
/// <param name="leftLanePath">lane path to the left of vehicle</param>
/// <param name="rightLanePath">lane path to the right of vehicle</param>
/// <param name="observedObstacles">static obstacles</param>
/// <param name="observedVehicles">observed vehicles</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="forwardPath">forward path</param>
public void ForwardPlanSimple(Path currentLanePath, Path leftLanePath, Path rightLanePath,
ObservedObstacles observedObstacles,
ObservedVehicle[] observedVehicles,
Coordinates position, Coordinates heading, double speed,
out AboutPath aboutPath, out Path forwardPath)
{
// set up vehicle and lane information
InitialiseInformation(position, heading, speed, leftLanePath, currentLanePath, rightLanePath);
// manage static and dynamic dynamic obstacles
InitialiseObstacles(leftLanePath, currentLanePath, rightLanePath,
observedObstacles, observedVehicles);
double projectionDist = Math.Max(vehicleSpeed * 3, 10) + TahoeParams.FL;
double origProjectionDist = projectionDist;
double pathRisk, pathRiskDist, pathSepDist;
do {
// lookahead point
double lookaheadDist = projectionDist;
PointOnPath lookaheadPt = currentLanePath.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
forwardPath = GenerateBezierPath(startPoint, endPoint, startVec, endVec);
// determine risk of spline path
CheckPathRisk(forwardPath, out pathRisk, out pathRiskDist, out pathSepDist);
if (pathRisk != 0)
projectionDist = Math.Max(pathRiskDist - 1, 0);
} while (pathRisk != 0 && projectionDist != 0);
if (pathRisk == 0 && projectionDist == origProjectionDist)
aboutPath = AboutPath.Normal;
else if (projectionDist != 0)
aboutPath = AboutPath.Stop;
else
aboutPath = AboutPath.Null;
}
/// <summary>
/// Reason about travelling the lane ahead
/// </summary>
/// <param name="currentLanePath">lane path that vehicle is following</param>
/// <param name="leftLanePath">lane path to the left of vehicle</param>
/// <param name="rightLanePath">lane path to the right of vehicle</param>
/// <param name="observedObstacles">static obstacles</param>
/// <param name="observedVehicles">observed vehicles</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="forwardPath">forward path</param>
public void ForwardPlan(Path currentLanePath, Path leftLanePath, Path rightLanePath,
ObservedObstacles observedObstacles,
ObservedVehicle[] observedVehicles,
Coordinates position, Coordinates heading, double speed,
out AboutPath aboutPath, out Path forwardPath)
{
// set up vehicle and lane information
InitialiseInformation(position, heading, speed, leftLanePath, currentLanePath, rightLanePath);
// manage static and dynamic dynamic obstacles
InitialiseObstacles(leftLanePath, currentLanePath, rightLanePath,
observedObstacles, observedVehicles);
double projectionDist = Math.Max(vehicleSpeed * 3, 10) + TahoeParams.FL;
double origProjectionDist = projectionDist;
double spacing = 0.4;
int numPaths = (int)Math.Round(currentLaneWidth / spacing);
numPaths -= (int)Math.IEEERemainder((double)numPaths, 2.0);
int midPathIndex = (numPaths - 1) / 2;
int selectedPathIndex;
double[] pathsRisk = new double[numPaths];
double[] pathsRiskDist = new double[numPaths];
double[] pathsSepDist = new double[numPaths];
double[] pathsCost = new double[numPaths];
Path[] paths = new Path[numPaths];
do {
// lookahead point
double lookaheadDist = projectionDist;
PointOnPath lookaheadPt = currentLanePath.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));
Coordinates rVec = endVec.Rotate90();
// generate multiple spline paths and evaluate their risks
for (int i = 0; i < numPaths; i++) {
// generate spline path
paths[i] = GenerateBezierPath(startPoint, endPoint + rVec.Normalize((i - midPathIndex) * spacing), startVec, endVec);
// determine risk of spline path
CheckPathRisk(paths[i], out pathsRisk[i], out pathsRiskDist[i], out pathsSepDist[i]);
}
// find minimum path risk (0 means it is a safe path, non-zero means it has some risk
double minPathRisk = -1;
for (int i = 0; i < numPaths; i++) {
pathsRisk[i] = Math.Round(pathsRisk[i], 3);
if (pathsRisk[i] < minPathRisk || minPathRisk == -1)
minPathRisk = pathsRisk[i];
}
// select candidate paths and set up their cost
for (int i = 0; i < numPaths; i++) {
if (pathsRisk[i] == minPathRisk)
pathsCost[i] = 0;
else
pathsCost[i] = -1;
}
// find cost of candidate paths
double weightDev = 5; // weight for path deviation penalty
double weightDir = 1; // weight for left path penalty
for (int i = 0; i < numPaths; i++) {
// skip paths with risk in first spline
if (pathsCost[i] < 0)
continue;
double dir;
if (i < midPathIndex)
dir = 1;
else
dir = 0;
pathsCost[i] = weightDev * Math.Abs(i - midPathIndex) +
weightDir * dir;
}
// find index of path to select
selectedPathIndex = -1;
double minPathCost = -1;
for (int i = 0; i < numPaths; i++) {
if (pathsCost[i] < 0)
continue;
if (pathsCost[i] < minPathCost || minPathCost == -1) {
selectedPathIndex = i;
minPathCost = pathsCost[i];
}
}
if (pathsRisk[selectedPathIndex] != 0)
projectionDist = Math.Max(pathsRiskDist[selectedPathIndex] - 1, 0);
} while (pathsRisk[selectedPathIndex] != 0 && projectionDist > 7.5);
// prepare safest path
forwardPath = new Path();
forwardPath.Add((BezierPathSegment)(paths[selectedPathIndex][0]));
if (pathsRisk[selectedPathIndex] == 0)
aboutPath = AboutPath.Normal;
else if (projectionDist != 0)
aboutPath = AboutPath.Stop;
else
aboutPath = AboutPath.Null;
}
/// <summary>
/// Set dynamic obstacles given in absolute coordinates (Version 1)
/// </summary>
/// <param name="obstacles"></param>
public void SetDynamicObstaclesVer1(Path observedVehiclePath, ObservedVehicle[] observedVehicles)
{
// generate paths for
for (int i = 0; i < observedVehicles.Length; i++) {
// add observed vehicle
dynamicObstacles.Add(observedVehicles[i]);
// lane position
PointOnPath observedVehiclePosition = observedVehiclePath.GetClosest(observedVehicles[i].AbsolutePosition);
// lookahead point
double projectionDist = Math.Max(vehicleSpeed * 3, 10) + TahoeParams.FL;
double lookaheadDist = projectionDist;
PointOnPath projectionPoint = observedVehiclePath.AdvancePoint(observedVehiclePosition, ref lookaheadDist);
// extend point if at end of path
Coordinates offsetVec = new Coordinates(0, 0);
if (lookaheadDist > 0.5)
offsetVec = projectionPoint.segment.Tangent(projectionPoint).Normalize(lookaheadDist);
// prepare ctrl points for spline path
Coordinates startPoint = observedVehicles[i].AbsolutePosition;
Coordinates endPoint = projectionPoint.pt + offsetVec;
Coordinates startVec = observedVehiclePosition.segment.Tangent(observedVehiclePosition).Normalize(Math.Max(observedVehicles[i].Speed, 2.0));
Coordinates endVec = projectionPoint.segment.Tangent(projectionPoint).Normalize(Math.Max(observedVehicles[i].Speed, 2.0));
// generate spline path
dynamicObstaclesPaths.Add(GenerateBezierPath(startPoint, endPoint, startVec, endVec));
// generate static obstacles if speed is close to zero
if (observedVehicles[i].Speed < 1.0) {
Coordinates tVec = observedVehiclePosition.segment.Tangent(observedVehiclePosition).Normalize(observedVehicles[i].Length / 2);
Coordinates rVec = observedVehiclePosition.segment.Tangent(observedVehiclePosition).Rotate90().Normalize(observedVehicles[i].Width / 2);
staticObstaclesIn.Add(observedVehicles[i].AbsolutePosition + tVec + rVec); // front left
staticObstaclesIn.Add(observedVehicles[i].AbsolutePosition + tVec - rVec); // front right
staticObstaclesIn.Add(observedVehicles[i].AbsolutePosition - tVec + rVec); // rear left
staticObstaclesIn.Add(observedVehicles[i].AbsolutePosition - tVec - rVec); // rear right
}
}
}
/// <summary>
/// Set dynamic obstacles given in absolute coordinates (Version 2a)
/// </summary>
/// <param name="obstacles"></param>
public void SetDynamicObstacles(Path observedVehiclePath, ObservedVehicle[] observedVehicles)
{
// generate paths for
for (int i = 0; i < observedVehicles.Length; i++) {
// add observed vehicle
dynamicObstacles.Add(observedVehicles[i]);
// lane position
PointOnPath observedVehicleLanePosition = observedVehiclePath.GetClosest(observedVehicles[i].AbsolutePosition);
// lookahead point
double projectionDist = Math.Max(observedVehicles[i].Speed * 5, 10) + 0.5 * TahoeParams.VL;
double lookaheadDist = projectionDist;
PointOnPath projectionPoint = observedVehiclePath.AdvancePoint(observedVehicleLanePosition, ref lookaheadDist);
// extend point if at end of path
Coordinates offsetVec = new Coordinates(0, 0);
if (lookaheadDist > 0.5)
offsetVec = projectionPoint.segment.Tangent(projectionPoint).Normalize(lookaheadDist);
// prepare ctrl points for spline path
Coordinates startPoint = observedVehicles[i].AbsolutePosition;
Coordinates endPoint = projectionPoint.pt + offsetVec;
Coordinates startVec = observedVehicleLanePosition.segment.Tangent(observedVehicleLanePosition).Normalize(Math.Max(observedVehicles[i].Speed, 2.0));
Coordinates endVec = projectionPoint.segment.Tangent(projectionPoint).Normalize(Math.Max(observedVehicles[i].Speed, 2.0));
// generate mid points of path
int midPointsTotal = (int)Math.Round(projectionDist / 5.0) - 1;
double midPointStepDist = projectionDist / (midPointsTotal + 1);
Coordinates[] midPoints = new Coordinates[midPointsTotal];
Coordinates[] midVecs = new Coordinates[midPointsTotal];
Coordinates[] midShiftVecs = new Coordinates[midPointsTotal];
for (int j = 0; j < midPointsTotal; j++) {
// lookahead point
lookaheadDist = projectionDist * (j + 1) / (midPointsTotal + 1);
projectionPoint = observedVehiclePath.AdvancePoint(observedVehicleLanePosition, ref lookaheadDist);
// extend point if at end of path
offsetVec = new Coordinates(0, 0);
if (lookaheadDist > 0.5)
offsetVec = projectionPoint.segment.Tangent(projectionPoint).Normalize(lookaheadDist);
// prepare ctrl points for spline path
midPoints[j] = projectionPoint.pt + offsetVec;
midVecs[j] = projectionPoint.segment.Tangent(projectionPoint).Normalize(Math.Max(vehicleSpeed, 2.0));
midShiftVecs[j] = midVecs[j].Rotate90();
}
// vehicle vector with respect to segment closest point
Coordinates carVec = observedVehicles[i].AbsolutePosition - observedVehicleLanePosition.pt;
// segment tangent vector
Coordinates pathVec = observedVehicleLanePosition.segment.Tangent(observedVehicleLanePosition);
// compute offtrack error
double offtrackError = Math.Sign(carVec.Cross(pathVec)) * observedVehicleLanePosition.pt.DistanceTo(observedVehicles[i].AbsolutePosition);
// path points
Coordinates[] pathPoints = new Coordinates[midPointsTotal + 2];
pathPoints[0] = startPoint;
pathPoints[midPointsTotal + 1] = endPoint;
for (int j = 0; j < midPointsTotal; j++) {
double control = 0.0;
if (j == 0)
control = 0.35;
else if (j == midPointsTotal - 1)
control = -0.35;
pathPoints[j + 1] = midPoints[j] - midShiftVecs[j].Normalize(offtrackError *
(midPointsTotal - j + control) /
(midPointsTotal + 1));
}
// generate spline path with points
Path projectedPath = new Path();
CubicBezier[] beziers = SmoothingSpline.BuildC2Spline(pathPoints, startVec.Normalize(0.5 * midPointStepDist),
endVec.Normalize(0.5 * midPointStepDist), 0.5);
for (int j = 0; j < beziers.Length; j++) {
projectedPath.Add(new BezierPathSegment(beziers[j], (double?)null, false));
}
// generate spline path
dynamicObstaclesPaths.Add(projectedPath);
// generate static obstacles if speed is close to zero
if (observedVehicles[i].Speed < 1.0) {
Coordinates tVec = observedVehicleLanePosition.segment.Tangent(observedVehicleLanePosition).Normalize(observedVehicles[i].Length / 2);
Coordinates rVec = observedVehicleLanePosition.segment.Tangent(observedVehicleLanePosition).Rotate90().Normalize(observedVehicles[i].Width / 2);
staticObstaclesIn.Add(observedVehicles[i].AbsolutePosition + tVec + rVec); // front left
staticObstaclesIn.Add(observedVehicles[i].AbsolutePosition + tVec - rVec); // front right
staticObstaclesIn.Add(observedVehicles[i].AbsolutePosition - tVec + rVec); // rear left
staticObstaclesIn.Add(observedVehicles[i].AbsolutePosition - tVec - rVec); // rear right
}
}
}
/// <summary>
/// Generates adjacency of a partition to another lane
/// </summary>
/// <param name="alp"></param>
/// <param name="al"></param>
private void GenerateSinglePartitionAdjacency(ArbiterLanePartition alp, ArbiterLane al)
{
// fake path
List<IPathSegment> fakePathSegments = new List<IPathSegment>();
fakePathSegments.Add(new LinePathSegment(alp.Initial.Position, alp.Final.Position));
Path fakePath = new Path(fakePathSegments);
// partitions adjacent
List<ArbiterLanePartition> adjacent = new List<ArbiterLanePartition>();
// tracks
PointOnPath current = fakePath.StartPoint;
double increment = 0.5;
double tmpInc = 0;
// increment along
while (tmpInc == 0)
{
// loop over partitions
foreach (ArbiterLanePartition alpar in al.Partitions)
{
// get fake path for partition
List<IPathSegment> ips = new List<IPathSegment>();
ips.Add(new LinePathSegment(alpar.Initial.Position, alpar.Final.Position));
Path alpPath = new Path(ips);
// get closest point on tmp partition to current
PointOnPath closest = alpPath.GetClosest(current.pt);
// check general distance
if (closest.pt.DistanceTo(current.pt) < 20)
{
// check not start or end
if (!closest.Equals(alpPath.StartPoint) && !closest.Equals(alpPath.EndPoint))
{
// check not already added
if (!adjacent.Contains(alpar))
{
// add
adjacent.Add(alpar);
}
}
}
}
// set inc
tmpInc = increment;
// increment point
current = fakePath.AdvancePoint(current, ref tmpInc);
}
// add adjacent
alp.NonLaneAdjacentPartitions.AddRange(adjacent);
}