/// <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;
}
public void AvoidancePath(Path currentLanePath, Path leftLanePath, Path rightLanePath,
ObservedObstacles observedObstacles,
ObservedVehicle[] observedVehicles,
Coordinates position, Coordinates heading, double speed,
out AboutPath aboutPath, out Path avoidancePath)
{
// set up vehicle and lane information
InitialiseInformation(position, heading, speed, leftLanePath, currentLanePath, rightLanePath);
// manage static and dynamic dynamic obstacles
//SetDynamicObstacles(observedVehicles);
SetStaticObstacles(currentLanePath, observedObstacles);
// obtain points along the path and their tangents
List<Coordinates> pathPoints, pathPointTangents;
List<double> pathPointDistances;
GetPointsOnPath(currentLanePath, currentLanePosition, 25, 1,
out pathPoints, out pathPointTangents, out pathPointDistances);
int binTotal = 41; // odd
int binMidIndex = (binTotal - 1) / 2;
double binSize = 0.2;
double binRange = binTotal * binSize;
int[] binSelectedIndexes = new int[pathPoints.Count];
Coordinates[] pathShifts = new Coordinates[pathPoints.Count];
for (int i = 0; i < pathPoints.Count; i++) {
double[] bins = new double[binTotal];
foreach (Coordinates obstacle in staticObstaclesIn) {
if (Math.Abs(pathPoints[i].X - obstacle.X) > 5.0 ||
Math.Abs(pathPoints[i].Y - obstacle.Y) > 5.0)
continue;
Coordinates obsTf = obstacle - pathPoints[i];
obsTf = obsTf.Rotate(-pathPointTangents[i].ArcTan);
if (Math.Abs(obsTf.Y) < 4.0 && Math.Abs(obsTf.X) < 4.0)
bins[(int)Math.Round(-obsTf.Y / binSize) + binMidIndex] += 1;
}
double[] riskBins = new double[binTotal];
for (int j = 0; j < binTotal; j++) {
for (int k = Math.Max(j-10, 0); k < Math.Min(j+10, binTotal); k++) {
riskBins[j] += bins[k];
}
}
List<int> candidateBinIndexes = new List<int>();
for (int j = 0; j < binTotal; j++) {
if (riskBins[j] == 0)
candidateBinIndexes.Add(j);
}
double weightDev = 5; // weight for path deviation penalty
double weightDir = 1; // weight for left path penalty
List<double> costBins = new List<double>();
foreach (int candidateBinIndex in candidateBinIndexes) {
double dir;
if (candidateBinIndex < binMidIndex)
dir = 1;
else
dir = 0;
costBins.Add(weightDev * Math.Abs(candidateBinIndex - binMidIndex) +
weightDir * dir);
}
binSelectedIndexes[i] = -1;
double minCost = -1;
for (int j = 0; j < candidateBinIndexes.Count; j++) {
if (costBins[j] < minCost || minCost == -1) {
binSelectedIndexes[i] = candidateBinIndexes[j];
minCost = costBins[j];
}
}
pathShifts[i] += binSize * (binMidIndex - binSelectedIndexes[i]) * pathPointTangents[i].RotateM90().Normalize();
}
for (int i = 1; i < pathPoints.Count-1; i++) {
if (binSelectedIndexes[i] != binSelectedIndexes[i - 1] && binSelectedIndexes[i] != binSelectedIndexes[i + 1])
binSelectedIndexes[i] = binSelectedIndexes[i - 1];
}
for (int i = 0; i < pathPoints.Count; i++) {
pathPoints[i] += binSize * (binSelectedIndexes[i] - binMidIndex) * pathPointTangents[i].RotateM90().Normalize();
}
// generate path with points
CubicBezier[] beziers = SmoothingSpline.BuildC2Spline(pathPoints.ToArray(), null, null, 0.5);
avoidancePath = new Path();
for (int i = 0; i < beziers.Length; i++) {
avoidancePath.Add(new BezierPathSegment(beziers[i], (double?)null, false));
}
aboutPath = AboutPath.Normal;
}
/// <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 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 static obstacles given as vectors relative from imu
/// </summary>
/// <param name="obstacles">observed obstacles in relative vectors from imu</param>
private void SetStaticObstacles(Path path, ObservedObstacles observedObstacles)
{
// clear static obstacles
staticObstaclesIn.Clear();
staticObstaclesOut.Clear();
staticObstaclesFake.Clear();
// determine sensor region to group obstacles
DefineSensorRegion(path);
List<PointOnPath> pop = new List<PointOnPath>();
// prepare static obstacles for obstacle reasoning
for (int i = 0; i < observedObstacles.Obstacles.Length; i++) {
// transform obstacle to absolute coordinates
Coordinates obs = new Coordinates(TahoeParams.IL, 0);
obs += observedObstacles.Obstacles[i].ObstacleVector;
obs = obs.Rotate(vehicleHeading) + vehiclePosition;
// sort out static obstacles
if (sensorPolygon.IsInside(obs) == true)
staticObstaclesIn.Add(obs);
else
staticObstaclesOut.Add(obs);
}
}
/// <summary>
/// Manage obstacle information
/// </summary>
/// <param name="currentPath"></param>
/// <param name="observedObstacles"></param>
/// <param name="currentLaneObservedVehicles"></param>
/// <param name="leftLaneObservedVehicles"></param>
/// <param name="rightLaneObservedVehicles"></param>
private void InitialiseObstacles(Path leftPath, Path currentPath, Path rightPath,
ObservedObstacles observedObstacles,
ObservedVehicle[] observedVehicles)
{
//SetDynamicObstacles(observedVehicles);
SetStaticObstacles(currentPath, observedObstacles);
}
