public ObservedVehicleDisplay(ObservedVehicle observedVehicle, Color c)
{
// set the vehicle
this.observedVehicle = observedVehicle;
color = c;
bodyRect = new RectangleF(-Width / 2, -RearOffset, Width, Length);
wheelRectL = RectangleF.FromLTRB(-tireWidth, -tireDiameter / 2, 0, tireDiameter / 2);
wheelRectR = RectangleF.FromLTRB(0, -tireDiameter / 2, tireWidth, tireDiameter / 2);
}
private RectangleF wheelRectR, wheelRectL; // left and right wheel rectangle
#endregion Fields
#region Constructors
/// <summary>
/// Full Constructor
/// </summary>
/// <param name="Position"></param>
/// <param name="velocity"></param>
public ObservedVehicleDisplay(ObservedVehicle observedVehicle)
{
// set the vehicle
this.observedVehicle = observedVehicle;
// set the vehicle's color
if (observedVehicle.ObservationState == ObservedVehicleState.Normal)
{
color = Color.Green;
}
else if (observedVehicle.ObservationState == ObservedVehicleState.Occluded)
{
color = Color.Red;
}
else
{
color = Color.Black;
}
bodyRect = new RectangleF(-Width / 2, -RearOffset, Width, Length);
wheelRectL = RectangleF.FromLTRB(-tireWidth, -tireDiameter / 2, 0, tireDiameter / 2);
wheelRectR = RectangleF.FromLTRB(0, -tireDiameter / 2, tireWidth, tireDiameter / 2);
}
/// <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>
/// Produces an obstacle reasoning path for an intersection situation
/// </summary>
/// <param name="originalTurnPath">The default turn path for the turn</param>
/// <param name="entryAdjacentLanePath">The path defining the lane that is adjacent to the entry lane</param>
/// <param name="entryPath">The path we will follow in the entry lane</param>
/// <param name="entryAdjacentLaneVehicles">The vehicles in the lane adjacent to the entry we are traveling to</param>
/// <param name="vehicleState">Our current vehicle state</param>
/// <returns>A modified turn path to follow that avoids the obstacles in the entry's adjacent lane</returns>
public Path IntersectionObstacleReasoning(Path originalTurnPath,
Path entryAdjacentLanePath, Path entryPath,
ObservedVehicle[] entryAdjacentLaneVehicles,
VehicleState vehicleState)
{
// set up vehicle and lane information
InitialiseInformation(vehicleState.xyPosition, vehicleState.heading, vehicleState.speed,
null, originalTurnPath, null);
// set up static obstacles (none for now)
staticObstaclesIn.Clear();
staticObstaclesOut.Clear();
staticObstaclesFake.Clear();
// set up dynamic obstacles
dynamicObstacles.Clear();
dynamicObstaclesPaths.Clear();
SetDynamicObstacles(entryAdjacentLanePath, entryAdjacentLaneVehicles);
// 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];
// path shift vector
Coordinates pathStartVec = originalTurnPath[0].Tangent(originalTurnPath.StartPoint);
Coordinates pathEndVec = originalTurnPath[originalTurnPath.Count - 1].Tangent(originalTurnPath.EndPoint);
Coordinates shiftVec = Math.Sign(pathEndVec.Cross(pathStartVec)) * originalTurnPath[0].Tangent(originalTurnPath.StartPoint);
// generate multiple paths
for (int i = 0; i < numPaths; i++) {
// determine path shift vector
Coordinates sVec = shiftVec.Normalize((i - midPathIndex) * spacing);
// generate path
paths[i] = new Path();
BezierPathSegment bezSeg = (BezierPathSegment)originalTurnPath[0];
paths[i].Add(new BezierPathSegment(bezSeg.cb.P0, bezSeg.cb.P1,
bezSeg.cb.P2 - sVec, bezSeg.cb.P3 - sVec, (double?)null, false));
for (int j = 1; j < originalTurnPath.Count; j++) {
bezSeg = (BezierPathSegment)originalTurnPath[j];
paths[i].Add(new BezierPathSegment(bezSeg.cb.P0 - sVec, bezSeg.cb.P1 - sVec,
bezSeg.cb.P2 - sVec, bezSeg.cb.P3 - sVec, (double?)null, false));
}
}
// evaluate paths and select safest path
int selectedPathIndex = EvaluatePaths(paths, midPathIndex, out pathsRisk, out pathsRiskDist, out pathsSepDist, out pathsCost);
// 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>
/// 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>
/// 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>
/// Runs an inference cycle over the intersection incrementing the timers by dt
/// </summary>
/// <param name="dt"></param>
/// <param name="ai">interconnect we are looking at proceeding through</param>
public void Update(double dt, ObservedVehicle[] vehicles)
{
// 1. Assign or remove vehicles given new sensor data
// 2. Priority lanes we get closest vehicle to intersection exit of that priority lane and determine if we are monitoring that
// 3. For all priority montiors, update time waiting by dt
// 4. determine state of intersection and which exits are blocked, update failure state of interconnects that are blocked to be feasible
// 4. Determine for all if should go or not
// 5. For all that should go and are not moving increase failure time by dt (not that our exit or lane never fails)
}
/// <summary>
/// Determines if a vehicle is contained inside a polygon
/// With a distance epsilon allowed for error in our polygon bounds (i.e. shrink polygon by epsilon)
/// With a factor portion of the vehicle's total area needed to be inside bounds to return true
/// </summary>
/// <param name="vehicle">Vehicle in question</param>
/// <param name="polygon">Polygon to check the vehicle against</param>
/// <param name="portion">Portion of the area (greater than 0 less than or equal to 1) that needs to be inside polygon to return true</param>
/// <param name="epsilon">Epsilon in meters to shrink the polygon by</param>
/// <returns></returns>
/// <remarks>Careful of vehicle with 0 width of length. Just use position in that case</remarks>
/// <remarks>Assuming that a given boundary line shares one coordinate with the next boundary line in the list</remarks>
public static bool CheckVehicleInPolygon(ObservedVehicle vehicle, double portion, List<BoundaryLine> polygon, double epsilon)
{
/*//resize polygon
//find center
Coordinates center = new Coordinates(0f, 0f);
foreach (BoundaryLine bl in polygon)
{
center += bl.p1 + bl.p2;
}
//since each point is present exactly twice in the boundary line representation
center.X = center.X / 2 * polygon.Count;
center.Y = center.Y / 2 * polygon.Count;
//adjust polygon and check for line segment intersections with the vehicle, maintaining a list for such intersections
Coordinates vehicleCenter = recalculateCenter(vehicle, 0);
Rectangle vehicleRect = calculateRectangle(vehicleCenter, vehicle.Width, vehicle.Length, vehicle.Heading.ToDegrees() * Math.PI / 180);
List<Coordinates> vehicleHits = new List<Coordinates>();
foreach (BoundaryLine bl in polygon)
{
bl.p1 = resizeCoordinate(center, bl.p1, epsilon);
bl.p2 = resizeCoordinate(center, bl.p2, epsilon);
Coordinates? up = LineIntersectsLine(vehicleRect.upperLeft, vehicleRect.upperRight, bl.p1, bl.p2);
Coordinates? down = LineIntersectsLine(vehicleRect.lowerLeft, vehicleRect.lowerRight, bl.p1, bl.p2);
Coordinates? left = LineIntersectsLine(vehicleRect.upperLeft, vehicleRect.lowerLeft, bl.p1, bl.p2);
Coordinates? right = LineIntersectsLine(vehicleRect.upperRight, vehicleRect.lowerRight, bl.p1, bl.p2);
if (up != null) vehicleHits.Add((Coordinates)up);
if (down != null) vehicleHits.Add((Coordinates)down);
if (left != null) vehicleHits.Add((Coordinates)left);
if (right != null) vehicleHits.Add((Coordinates)right);
}
if (vehicleHits.Count == 0)
{
//vehicle is either entirely inside or entirely outside the polygon
foreach (BoundaryLine bl in polygon)
{
if (LineIntersectsLine(center, vehicleRect.lowerLeft, bl.p1, bl.p2) != null) return false;
}
return true;
}
bool bUpperLeft = false; bool bUpperRight = false; bool bLowerLeft = false; bool bLowerRight = false;
foreach (BoundaryLine bl in polygon)
{
if (LineIntersectsLine(center, vehicleRect.lowerLeft, bl.p1, bl.p2) != null) bLowerLeft = true;
if (LineIntersectsLine(center, vehicleRect.lowerRight, bl.p1, bl.p2) != null) bLowerRight = true;
if (LineIntersectsLine(center, vehicleRect.upperLeft, bl.p1, bl.p2) != null) bUpperLeft = true;
if (LineIntersectsLine(center, vehicleRect.upperRight, bl.p1, bl.p2) != null) bUpperRight = true;
}
if (bLowerLeft) vehicleHits.Add(vehicleRect.lowerLeft);
if (bLowerRight) vehicleHits.Add(vehicleRect.lowerRight);
if (bUpperLeft) vehicleHits.Add(vehicleRect.upperLeft);
if (bUpperRight) vehicleHits.Add(vehicleRect.upperRight);
List<BoundaryLine> vehiclePolygon = JarvisMarch(vehicleHits);
//need to calculate the area of the vehicle inside the polygon
double area = 0;
foreach (BoundaryLine bl in vehiclePolygon)
{
area += TriangleArea(vehicleCenter, bl.p1, bl.p2);
}
if ((area / (vehicle.Length * vehicle.Width)) > portion) return true;
return false;*/
return false;
}
/// <summary>
/// Checks if a specific vehicle is within generous vehicle-like area of an exit
/// </summary>
/// <param name="observedVehicle"></param>
/// <param name="stop"></param>
/// <returns></returns>
public static bool CheckVehicleAtExit(ObservedVehicle observedVehicle, RndfWayPoint exit)
{
throw new Exception("This method is not yet implemented");
}
/// <summary>
/// Calculates vehicle's rectangular center due to separation requirements and back axel position
/// </summary>
/// <param name="svs">Vehicle</param>
/// <param name="extraForwardSpace">Extra space needed in the front</param>
/// <returns></returns>
public static Coordinates recalculateCenter(ObservedVehicle svs, double extraForwardSpace)
{
//double addedLength = (2 * svs.Length + extraForwardSpace + 1) / 2 - svs.PositionOffsetFromRear - 1;
//return (svs.AbsolutePosition + new Coordinates(addedLength * Math.Cos(svs.Heading.ToDegrees() * Math.PI / 180),
// addedLength * Math.Sin(svs.Heading.ToDegrees() * Math.PI / 180)));
return new Coordinates();
}
/// <summary>
/// Checks if a specific vehicle is within the intersection itself
/// </summary>
/// <param name="stops"></param>
/// <param name="intersectionPolygon"></param>
/// <returns></returns>
public static bool CheckVehicleWithinIntersection(ObservedVehicle observedVehicle, List<BoundaryLine> intersectionPolygon)
{
return CheckVehicleInPolygon(observedVehicle, 1, intersectionPolygon, 0);
}
/// <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>
/// 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>
/// 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>
/// 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);
}