/// <summary>
/// walks along the bezier path and creates a straight line segment approximation
/// </summary>
/// <param name="path"></param>
/// <param name="sampleDistance"></param>
/// <returns></returns>
public PointPath ConvertBezierPathToPointPath(IPath path, double sampleDistance)
{
PointPath pathOut = new PointPath();
foreach (IPathSegment seg in path)
{
if (seg is BezierPathSegment == false)
{
throw new InvalidOperationException();
}
BezierPathSegment bseg = seg as BezierPathSegment;
PointOnPath p = seg.StartPoint;
double refDist = 0;
while (refDist == 0)
{
PointOnPath p2;
refDist = sampleDistance;
p2 = seg.AdvancePoint(p, ref refDist);
pathOut.Add(new LinePathSegment(p.pt, p2.pt));
p = p2;
}
}
return(pathOut);
}
/// <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>
/// 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;
}
public SpeedControlData GetCommandedSpeed()
{
double alongTrackDist = CalculateAlongTrackDist(ZeroPoint, Coordinates.Zero);
double lookaheadDist = Math.Max(Services.StateProvider.GetVehicleState().speed *(TahoeParams.actuation_delay + 0.75), 1) + alongTrackDist;
PointOnPath pathPoint = AdvancePoint(ZeroPoint, lookaheadDist);
return(new SpeedControlData(GetSpeed(pathPoint), null));
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initialLane"></param>
/// <param name="finalLane"></param>
public ChangeLanesState(bool initialIsOncoming, LaneID initialLane, LaneID finalLane, Path changeLanesPath,
PointOnPath initialPosition, PointOnPath lowerBound, PointOnPath upperBound) : base(initialLane, finalLane)
{
this.initialIsOncoming = initialIsOncoming;
this.ChangeLanesPath = changeLanesPath;
this.InitialPosition = initialPosition;
this.LowerBound = lowerBound;
this.UpperBound = upperBound;
}
/// <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);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initialLane"></param>
/// <param name="finalLane"></param>
public ChangeLanesState(bool initialIsOncoming, LaneID initialLane, LaneID finalLane, Path changeLanesPath,
PointOnPath initialPosition, PointOnPath lowerBound, PointOnPath upperBound)
: base(initialLane, finalLane)
{
this.initialIsOncoming = initialIsOncoming;
this.ChangeLanesPath = changeLanesPath;
this.InitialPosition = initialPosition;
this.LowerBound = lowerBound;
this.UpperBound = upperBound;
}
public RobotPathMessage(int robotID, IPath path)
{
this.path = path;
this.robotID = robotID;
if (path.Count > 0)
{
this.goalPointOnPath = path[0].StartPoint;
this.robotPointOnPath = path[0].StartPoint;
}
}
public RobotPathMessage(int robotID, IPath path)
{
this.path = path;
this.robotID = robotID;
if (path.Count > 0)
{
this.goalPointOnPath = path[0].StartPoint;
this.robotPointOnPath = path[0].StartPoint;
}
}
private double GetCost(RobotTwoWheelCommand command)
{
double lookAheadRemaining = lookAhead;
double cost = 0;
PointOnPath closestPoint = path.GetClosest(state.Pose.ToVector2());
PointOnPath lookAheadPoint = path.AdvancePoint(closestPoint, ref lookAheadRemaining);
RobotTwoWheelState newState = RobotTwoWheelModel.Simulate(command, state, (lookAhead - lookAheadRemaining) / command.velocity);
return(newState.Pose.ToVector2().DistanceTo(lookAheadPoint.pt) * DISTANCE_WEIGHT); // - command.velocity * VELOCITY_WEIGHT - command.turn * TURN_WEIGHT;
}
public SteeringControlData GetSteeringControlData(SteeringControlDataOptions opts)
{
double alongTrackDist = CalculateAlongTrackDist(ZeroPoint, Coordinates.Zero);
double lookaheadDist = opts.PathLookahead + alongTrackDist;
PointOnPath pathPoint = AdvancePoint(ZeroPoint, lookaheadDist);
// send the path point
Services.UIService.PushPoint(pathPoint.Location, curTimestamp, "path point", true);
return(new SteeringControlData(GetCurvature(pathPoint), 0, 0));
}
public double GetCurvature(PointOnPath pt)
{
if (pt.Index >= Count - 1 || !pt.Valid)
{
throw new ArgumentOutOfRangeException();
}
else if (pt.Index == Count - 2)
{
return(GetCurvature(pt.Index));
}
else
{
return((1 - pt.DistFraction) * GetCurvature(pt.Index) + pt.DistFraction * GetCurvature(pt.Index + 1));
}
}
public void UpdatePath(IPath path)
{
if (path == null || path.Count == 0)
{
return;
}
double dist = 0.45;
PointOnPath lookAhead = path.AdvancePoint(path.StartPoint, ref dist);
Vector2 goalpointGlobal = lookAhead.pt;
double xg = (goalpointGlobal.X - currentPoint.x) * Math.Cos(currentPoint.yaw) + (goalpointGlobal.Y - currentPoint.y) * Math.Sin(currentPoint.yaw);
double yg = -(goalpointGlobal.X - currentPoint.x) * Math.Sin(currentPoint.yaw) + (goalpointGlobal.Y - currentPoint.y) * Math.Cos(currentPoint.yaw);
goalpoint = new Vector2(xg, yg);
}
public void UpdatePath(IPath path, LineSegment gap)
{
if (path == null || path.Count == 0)
{
return;
}
/*double dist = 0.45;
* PointOnPath lookAhead = path.AdvancePoint(path.StartPoint, ref dist);
*
* Vector2 goalpointGlobal = lookAhead.pt;
* double xg = (goalpointGlobal.X - currentPoint.x) * Math.Cos(currentPoint.yaw) + (goalpointGlobal.Y - currentPoint.y) * Math.Sin(currentPoint.yaw);
* double yg = -(goalpointGlobal.X - currentPoint.x) * Math.Sin(currentPoint.yaw) + (goalpointGlobal.Y - currentPoint.y) * Math.Cos(currentPoint.yaw);
* goalpoint = new Vector2(xg, yg);*/
double dist = 1.5;
PointOnPath lookAhead = path.AdvancePoint(path.StartPoint, ref dist);
Vector2 mean = new Vector2((gap.P0.X + gap.P1.X) / 2, (gap.P0.Y + gap.P1.Y) / 2);
double theta = Math.Atan2(gap.P0.Y - gap.P1.Y, gap.P0.X - gap.P1.X);
Vector2 goal1 = mean + 0.25 * (new Vector2(Math.Cos(theta + Math.PI / 2), Math.Sin(theta + Math.PI / 2)));
Vector2 goal2 = mean - 0.25 * (new Vector2(Math.Cos(theta + Math.PI / 2), Math.Sin(theta + Math.PI / 2)));
Vector2 goalpointGlobal = (goal1.DistanceTo(lookAhead.pt) < goal2.DistanceTo(lookAhead.pt)) ? goal1 : goal2;
double xg = (goalpointGlobal.X - currentPoint.x) * Math.Cos(currentPoint.yaw) + (goalpointGlobal.Y - currentPoint.y) * Math.Sin(currentPoint.yaw);
double yg = -(goalpointGlobal.X - currentPoint.x) * Math.Sin(currentPoint.yaw) + (goalpointGlobal.Y - currentPoint.y) * Math.Cos(currentPoint.yaw);
goalpoint = new Vector2(xg, yg);
/*double dist = 1.5;
* PointOnPath lookAhead = path.AdvancePoint(path.StartPoint, ref dist);
*
* double goal1Y = (gap.P0.Y + gap.P1.Y) / 2 + (gap.P0.X - gap.P1.X) / (gap.Length) * 0.25;
* double goal1X = (gap.P0.X + gap.P1.X) / 2 + (gap.P0.Y - gap.P1.Y) / (gap.Length) * 0.25;
* Vector2 goal1 = new Vector2(goal1X, goal1Y);
*
* double goal2Y = (gap.P0.Y + gap.P1.Y) / 2 + (gap.P0.X - gap.P1.X) / (gap.Length) * -0.25;
* double goal2X = (gap.P0.X + gap.P1.X) / 2 + (gap.P0.Y - gap.P1.Y) / (gap.Length) * -0.25;
* Vector2 goal2 = new Vector2(goal2X, goal2Y);
*
* goalpoint = (goal1.DistanceTo(lookAhead.pt) < goal2.DistanceTo(lookAhead.pt)) ? goal1 : goal2;*/
}
/// <summary>
/// Gets closest lane to a point
/// </summary>
/// <param name="point"></param>
/// <returns></returns>
public ArbiterLane ClosestLane(Coordinates point)
{
ArbiterLane closest = null;
double best = Double.MaxValue;
foreach (ArbiterSegment asg in ArbiterSegments.Values)
{
foreach (ArbiterLane al in asg.Lanes.Values)
{
PointOnPath closestPoint = al.PartitionPath.GetClosest(point);
double tmp = closestPoint.pt.DistanceTo(point);
if (tmp < best)
{
closest = al;
best = tmp;
}
}
}
return(closest);
}
private Boolean IsBezSegmentClear(BezierPathSegment seg, List <Polygon> obstacles, double advanceDistance)
{
PointOnPath p = seg.StartPoint;
double refDist = 0;
while (refDist == 0)
{
PointOnPath p2;
refDist = advanceDistance;
p2 = seg.AdvancePoint(p, ref refDist);
foreach (Polygon poly in obstacles)
{
if (poly.ConvexDoesIntersect(p.pt, p2.pt))
{
return(false);
}
}
p = p2;
}
return(true);
}
/// <summary>
/// Check if the other line path intersects this line path
/// </summary>
/// <param name="other"></param>
/// <param name="tol"></param>
/// <returns></returns>
public bool Intersects(Path other, double tol)
{
// start point want to be outside of this lane
PointOnPath current = this.PartitionPath.StartPoint;
double increment = tol / 2.0;
double dist = 0;
while (dist == 0)
{
Coordinates pt = this.PartitionPath.GetClosest(current.pt).pt;
PointOnPath tmp = other.GetClosest(pt);
if (tmp.pt.DistanceTo(pt) <= tol)
{
return(true);
}
dist = increment;
current = this.PartitionPath.AdvancePoint(current, ref dist);
}
return(false);
}
public void UpdatePath(IPath path, LineSegment gap)
{
if (path == null || path.Count == 0 || pose == null)
{
return;
}
double dist = 1.5;
PointOnPath lookAhead = path.AdvancePoint(path.StartPoint, ref dist);
Vector2 mean = new Vector2((gap.P0.X + gap.P1.X) / 2, (gap.P0.Y + gap.P1.Y) / 2);
double theta = Math.Atan2(gap.P0.Y - gap.P1.Y, gap.P0.X - gap.P1.X) + Math.PI / 2;
Vector2 goal1 = mean + 0.25 * (new Vector2(Math.Cos(theta), Math.Sin(theta)));
Vector2 goal2 = mean - 0.25 * (new Vector2(Math.Cos(theta), Math.Sin(theta)));
Vector2 goalPointGlobal = ((goal1.X - lookAhead.pt.X) * (goal1.X - lookAhead.pt.X) + (goal1.Y - lookAhead.pt.Y) * (goal1.Y - lookAhead.pt.Y)
< (goal2.X - lookAhead.pt.X) * (goal2.X - lookAhead.pt.X) + (goal2.Y - lookAhead.pt.Y) * (goal2.Y - lookAhead.pt.Y)) ? goal1 : goal2;
double xg = (goalPointGlobal.X - pose.x) * Math.Cos(pose.yaw) + (goalPointGlobal.Y - pose.y) * Math.Sin(pose.yaw);
double yg = -(goalPointGlobal.X - pose.x) * Math.Sin(pose.yaw) + (goalPointGlobal.Y - pose.y) * Math.Cos(pose.yaw);
goalPoint = new Vector2(xg, yg);
}
public double GetAcceleration(PointOnPath pt)
{
return (speeds[pt.Index+1]*speeds[pt.Index+1]-speeds[pt.Index]*speeds[pt.Index])/(2*this[pt.Index].DistanceTo(this[pt.Index+1]));
}
private void PlanPurePursuit()
{
if (pathCurrentlyTracked == null)
{
return;
}
//we are really far off the path, just get on the stupid path
//mark sucks and wants this behavior
if (pathCurrentlyTracked.Length == 0)
{
goalPoint = pathCurrentlyTracked.EndPoint.pt;
}
else if (segmentCurrentlyTracked.DistanceOffPath(currentPoint.ToVector2()) > lookAheadDistParam / 2)
{
//Console.WriteLine("2");
double lookaheadRef = 0;
PointOnPath pTemp = segmentCurrentlyTracked.StartPoint;
goalPoint = pathCurrentlyTracked.AdvancePoint(pTemp, ref lookaheadRef).pt;
}
else
{
//Console.WriteLine("1");
//see if we can track the next segment and if so, update that...
PointOnPath currentPointOnPath = segmentCurrentlyTracked.ClosestPoint(currentPoint.ToVector2());
double lookaheadRef = lookAheadDistParam;
PointOnPath lookaheadPointOnPath = pathCurrentlyTracked.AdvancePoint(currentPointOnPath, ref lookaheadRef);
//the path lookahead point is at the end, and we cant do antyhing
segmentCurrentlyTracked = lookaheadPointOnPath.segment;
goalPoint = lookaheadPointOnPath.pt;
}
double errorX = (goalPoint.X - currentPoint.x);
double errorY = (goalPoint.Y - currentPoint.y);
Vector2 verr = new Vector2(errorX, errorY);
//Console.WriteLine(errorX + " | " + errorY);
double errorDistance = verr.Length;
double currentYaw = currentPoint.yaw;
double errorYaw = UnwrapAndCalculateYawError(errorX, errorY, ref currentYaw);
//Console.Write("neg Hyst: " + hysterisisNegative + " pos Hyst: " + hysterisisPositive + " yawError" + errorYaw * 180.0/ Math.PI+ " ||");
//the reason for this is that our angles are defined from 0 to 360
//but the PID controller expects angle to be -180 to 180
//calculate the control outputs
//the idea here is we want to make the velocity (which is a derivative) be proportional to the error in our actual
//position
double unsignedYawErrorNormalizeToOne = Math.Abs(errorYaw) / Math.PI;
double commandedVelocity = (kPPosition * errorDistance);
double commandedHeading = kPHeading * errorYaw;
if (Math.Abs(commandedVelocity) > capVelocity)
{
commandedVelocity = Math.Sign(commandedVelocity) * capVelocity;
}
if (Math.Abs(commandedHeading) > capHeadingDot)
{
commandedHeading = Math.Sign(commandedHeading) * capHeadingDot;
}
//if (unsignedYawErrorNormalizeToOne > .1)
// find which input to use
RobotTwoWheelCommand currentInput = inputList[pathCurrentlyTracked.IndexOf(segmentCurrentlyTracked)];
//commandedVelocity *= (1 - Math.Pow(unsignedYawErrorNormalizeToOne, velocityTurningDamingFactor));
//command = new RobotTwoWheelCommand(commandedVelocity, commandedHeading);
if (pathCurrentlyTracked.EndPoint.pt.DistanceTo(currentPoint.ToVector2()) < .2)
{
command = new RobotTwoWheelCommand(0, 0);
}
else
{
if (currentInput.velocity < 0.3)
{
command = new RobotTwoWheelCommand(0.4, commandedHeading);
}
else
{
command = new RobotTwoWheelCommand(currentInput.velocity, commandedHeading);
}
}
//if (reachedGoal)
//{
// command.velocity = 0;
// command.turn = 0;
//}
//Console.WriteLine("Current: " + currentPoint.x + " " + currentPoint.y + " " + currentPoint.yaw + " | " + errorDistance + " | " + errorYaw + " | " + commandedVelocity + " " + commandedHeading);
//Console.WriteLine();
}
public Coordinates Tangent(PointOnPath pt)
{
return (end - start).Normalize();
}
private double CalculateAlongTrackDist(PointOnPath pt, Coordinates loc)
{
return pt.AlongtrackDistance(loc);
}
public PointOnPath AdvancePoint(PointOnPath pt, ref double dist)
{
// get the angle of the point
double angle = (pt.pt - center).ArcTan;
// figure out where this sits in the scheme of things
if (ccw) {
// get it larger than start angle
if (angle < startAngle)
angle += 2*Math.PI;
// figure out if it's less than the end angle
if (angle > endAngle)
throw new InvalidOperationException();
// it's in the bounds, so calculate distance to end angle
double distToGo = radius*(endAngle-angle);
if (dist < distToGo) {
double newAngle = angle + dist / radius;
Vector2 pt2 = new Vector2(center.X + radius*Math.Cos(newAngle), center.Y + radius*Math.Sin(newAngle));
double dist2 = radius*(newAngle - startAngle);
dist = 0;
return new PointOnPath(this, dist2, pt2);
}
else {
dist -= distToGo;
return EndPoint;
}
}
else {
// get it smaller than the start angle
if (angle > startAngle)
angle -= 2*Math.PI;
// figure out oif it's smaller than the end angle
if (angle < endAngle)
throw new InvalidOperationException();
// it's in the bounds, so calculate distance to end angle
double distToGo = radius*(angle-endAngle);
if (dist < distToGo) {
double newAngle = angle - dist/radius;
Vector2 pt2 = new Vector2(center.X + radius*Math.Cos(newAngle), center.Y + radius*Math.Sin(newAngle));
double dist2 = radius*(startAngle - newAngle);
dist = 0;
return new PointOnPath(this, dist2, pt2);
}
else {
dist -= distToGo;
return EndPoint;
}
}
}
public double GetAcceleration(PointOnPath pt)
{
return((speeds[pt.Index + 1] * speeds[pt.Index + 1] - speeds[pt.Index] * speeds[pt.Index]) / (2 * this[pt.Index].DistanceTo(this[pt.Index + 1])));
}
/// <summary>
/// Reason about changing lanes
/// </summary>
/// <param name="previousChangeLanePath">previous change lane path</param>
/// <param name="initialLanePath">lane path that vehicle is changing from</param>
/// <param name="targetLanePath"> lane path that vehicle is changing to</param>
/// <param name="targetType"> type for target lane, left or right</param>
/// <param name="observedObstacles">static obstacles</param>
/// <param name="observedVehicles">observed vehicles</param>
/// <param name="lowerBound"> lower bound point on initial lane (similar to obstacle on target lane)</param>
/// <param name="upperBound"> upper bound point on initial lane (similar to obstacle on initial lane)</param>
/// <param name="position"> vehicle absolute position in m</param>
/// <param name="heading"> vehicle heading as a vector</param>
/// <param name="speed"> vehicle speed in m/s</param>
/// <param name="aboutPath"> type of path being returned</param>
/// <param name="currentChangeLanePath">change lane path</param>
public void LaneChangePlanAdvance(Path previousChangeLanePath,
Path initialLanePath, Path targetLanePath,
TargetLaneChangeType targetType,
ObservedObstacles observedObstacles,
ObservedVehicle[] observedVehicles,
PointOnPath initialLaneLowerBound, PointOnPath initialLaneUpperBound,
Coordinates position, Coordinates heading, double speed,
out AboutPath aboutPath, out Path currentChangeLanePath)
{
// check if target lane is to the left or right
if (targetType == TargetLaneChangeType.Left) {
// set up vehicle and lane information
InitialiseInformation(position, heading, speed, null, targetLanePath, initialLanePath);
// manage static and dynamic dynamic obstacles
InitialiseObstacles(null, targetLanePath, initialLanePath, observedObstacles, observedVehicles);
}
else {
// set up vehicle and lane information
InitialiseInformation(position, heading, speed, initialLanePath, targetLanePath, null);
// manage static and dynamic dynamic obstacles
InitialiseObstacles(initialLanePath, targetLanePath, null, observedObstacles, observedVehicles);
}
// determine risk of previous spline path, if provided
double pathRisk, pathRiskDist, pathSepDist;
if (previousChangeLanePath != null) {
// check risk of previous spline path
CheckPathRisk(previousChangeLanePath, out pathRisk, out pathRiskDist, out pathSepDist);
if (pathRisk == 0) {
// no risk was found, return previous spline path
currentChangeLanePath = previousChangeLanePath;
aboutPath = AboutPath.Normal;
return;
}
}
PointOnPath targetLaneLowerBound = targetLanePath.GetClosest(initialLaneLowerBound.pt);
PointOnPath targetLaneUpperBound = targetLanePath.GetClosest(initialLaneUpperBound.pt);
double targetLaneLowerBoundDist = Math.Round(targetLanePath.DistanceBetween(currentLanePosition, targetLaneLowerBound),1);
double targetLaneUpperBoundDist = Math.Round(targetLanePath.DistanceBetween(currentLanePosition, targetLaneUpperBound),1);
// generate obstacles for lower and upper bound points
Coordinates lowerBoundObstacle = targetLaneLowerBound.pt;
Coordinates upperBoundObstacle = initialLaneUpperBound.pt;
if (targetType == TargetLaneChangeType.Left) {
lowerBoundObstacle += targetLaneLowerBound.segment.Tangent(targetLaneLowerBound).RotateM90().Normalize(0.5 * currentLaneWidth - 1.0);
upperBoundObstacle += initialLaneUpperBound.segment.Tangent(initialLaneUpperBound).Rotate90().Normalize(0.5 * rightLaneWidth - 1.0);
}
else {
lowerBoundObstacle += targetLaneLowerBound.segment.Tangent(targetLaneLowerBound).Rotate90().Normalize(0.5 * currentLaneWidth - 1.0);
upperBoundObstacle += initialLaneUpperBound.segment.Tangent(initialLaneUpperBound).RotateM90().Normalize(0.5 * leftLaneWidth - 1.0);
}
staticObstaclesFake.Add(lowerBoundObstacle);
staticObstaclesFake.Add(upperBoundObstacle);
// path projection distance
double projectionDist = Math.Max(targetLaneLowerBoundDist, TahoeParams.VL + TahoeParams.FL);
double origProjectionDist = projectionDist;
do {
// lookahead point
double lookaheadDist = projectionDist;
PointOnPath lookaheadPt = targetLanePath.AdvancePoint(currentLanePosition, ref lookaheadDist);
// extend point if at end of path
Coordinates offsetVec = new Coordinates(0, 0);
if (lookaheadDist > 0.5)
offsetVec = lookaheadPt.segment.Tangent(lookaheadPt).Normalize(lookaheadDist);
// prepare ctrl points for spline path
Coordinates startPoint = vehiclePosition;
Coordinates endPoint = lookaheadPt.pt + offsetVec;
Coordinates startVec = new Coordinates(1, 0).Rotate(vehicleHeading).Normalize(Math.Max(vehicleSpeed, 2.0));
Coordinates endVec = lookaheadPt.segment.Tangent(lookaheadPt).Normalize(Math.Max(vehicleSpeed, 2.0));
// generate spline path
currentChangeLanePath = GenerateBezierPath(startPoint, endPoint, startVec, endVec);
// determine risk of spline path
CheckPathRisk(currentChangeLanePath, out pathRisk, out pathRiskDist, out pathSepDist);
// project further if current spline path has risk
if (pathRisk != 0) {
if (projectionDist == targetLaneUpperBoundDist + TahoeParams.RL)
break;
projectionDist = Math.Min(projectionDist + TahoeParams.VL / 2, targetLaneUpperBoundDist + TahoeParams.RL);
}
} while (pathRisk != 0 && projectionDist <= targetLaneUpperBoundDist + TahoeParams.RL);
// check if path without risk was found
if (pathRisk == 0)
aboutPath = AboutPath.Normal;
else
aboutPath = AboutPath.Null;
}
/// <summary>
/// Reason about changing lanes
/// </summary>
/// <param name="previousChangeLanePath">previous change lane path</param>
/// <param name="initialLanePath">lane path that vehicle is changing from</param>
/// <param name="targetLanePath"> lane path that vehicle is changing to</param>
/// <param name="targetType">type for target lane, left or right</param>
/// <param name="initialLaneVehicles">observed vehicles on initial lane</param>
/// <param name="initialLaneLowerBound">lower bound point on initial lane (similar to obstacle on target lane)</param>
/// <param name="initialLaneUpperBound">upper bound point on initial lane (similar to obstacle on initial lane)</param>
/// <param name="vehicleState">vehicle state</param>
public Path LaneChangeObstacleReasoning(Path previousChangeLanePath,
Path initialLanePath, Path targetLanePath,
TargetLaneChangeType targetType,
ObservedVehicle[] initialLaneVehicles,
PointOnPath initialLaneLowerBound,
PointOnPath initialLaneUpperBound,
VehicleState vehicleState)
{
// check if target lane is to the left or right
if (targetType == TargetLaneChangeType.Left) {
// set up vehicle and lane information
InitialiseInformation(vehicleState.xyPosition, vehicleState.heading, vehicleState.speed,
null, targetLanePath, initialLanePath);
}
else {
// set up vehicle and lane information
InitialiseInformation(vehicleState.xyPosition, vehicleState.heading, vehicleState.speed,
initialLanePath, targetLanePath, null);
}
// set up static obstacles (none for now)
staticObstaclesIn.Clear();
staticObstaclesOut.Clear();
staticObstaclesFake.Clear();
// set up dynamic obstacles
dynamicObstacles.Clear();
dynamicObstaclesPaths.Clear();
SetDynamicObstacles(initialLanePath, initialLaneVehicles);
// determine risk of previous spline path, if provided
double pathRisk, pathRiskDist, pathSepDist;
if (previousChangeLanePath != null) {
// check risk of previous spline path
CheckPathRisk(previousChangeLanePath, out pathRisk, out pathRiskDist, out pathSepDist);
// if no risk was found, return previous spline path
if (pathRisk == 0)
return previousChangeLanePath;
}
// set up number of paths based on lane width
double spacing = 0.25;
int numPaths = (int)Math.Round(currentLaneWidth / spacing);
if ((int)Math.IEEERemainder((double)numPaths, 2.0) == 0)
numPaths -= 1;
// increase number of drift paths
int midPathIndex;
numPaths += 12;
midPathIndex = (numPaths - 1) / 2;
double[] pathsRisk, pathsRiskDist, pathsSepDist, pathsCost;
Path[] paths = new Path[numPaths];
int selectedPathIndex;
PointOnPath targetLaneLowerBound = targetLanePath.GetClosest(initialLaneLowerBound.pt);
PointOnPath targetLaneUpperBound = targetLanePath.GetClosest(initialLaneUpperBound.pt);
double targetLaneLowerBoundDist = Math.Round(targetLanePath.DistanceBetween(currentLanePosition, targetLaneLowerBound), 1);
double targetLaneUpperBoundDist = Math.Round(targetLanePath.DistanceBetween(currentLanePosition, targetLaneUpperBound), 1);
// generate obstacles for lower and upper bound points
Coordinates lowerBoundObstacle = targetLaneLowerBound.pt;
Coordinates upperBoundObstacle = initialLaneUpperBound.pt;
if (targetType == TargetLaneChangeType.Left) {
lowerBoundObstacle += targetLaneLowerBound.segment.Tangent(targetLaneLowerBound).RotateM90().Normalize(0.5 * currentLaneWidth - 1.0);
upperBoundObstacle += initialLaneUpperBound.segment.Tangent(initialLaneUpperBound).Rotate90().Normalize(0.5 * rightLaneWidth - 1.0);
}
else {
lowerBoundObstacle += targetLaneLowerBound.segment.Tangent(targetLaneLowerBound).Rotate90().Normalize(0.5 * currentLaneWidth - 1.0);
upperBoundObstacle += initialLaneUpperBound.segment.Tangent(initialLaneUpperBound).RotateM90().Normalize(0.5 * leftLaneWidth - 1.0);
}
staticObstaclesFake.Add(lowerBoundObstacle);
staticObstaclesFake.Add(upperBoundObstacle);
// path projection distance
double projectionDist = Math.Max(targetLaneLowerBoundDist, TahoeParams.VL + TahoeParams.FL);
double origProjectionDist = projectionDist;
Path currentChangeLanePath = new Path();
do {
// lookahead point
double lookaheadDist = projectionDist;
PointOnPath lookaheadPt = targetLanePath.AdvancePoint(currentLanePosition, ref lookaheadDist);
// extend point if at end of path
Coordinates offsetVec = new Coordinates(0, 0);
if (lookaheadDist > 0.5)
offsetVec = lookaheadPt.segment.Tangent(lookaheadPt).Normalize(lookaheadDist);
// prepare ctrl points for first part of spline path
Coordinates startPoint = vehiclePosition;
Coordinates midPoint = lookaheadPt.pt + offsetVec;
Coordinates startVec = new Coordinates(1, 0).Rotate(vehicleHeading).Normalize(Math.Max(vehicleSpeed, 2.0));
Coordinates midVec = lookaheadPt.segment.Tangent(lookaheadPt).Normalize(Math.Max(vehicleSpeed, 2.0));
// lookahead point (for end point)
lookaheadDist = projectionDist + 10;
lookaheadPt = targetLanePath.AdvancePoint(currentLanePosition, ref lookaheadDist);
// extend point if at end of path (for end point)
offsetVec = new Coordinates(0, 0);
if (lookaheadDist > 0.5)
offsetVec = lookaheadPt.segment.Tangent(lookaheadPt).Normalize(lookaheadDist);
// prepare ctrl points for second part of spline path
Coordinates endPoint = lookaheadPt.pt + offsetVec;
Coordinates endVec = lookaheadPt.segment.Tangent(lookaheadPt).Normalize(Math.Max(vehicleSpeed, 2.0));
/////////////////////////////////
Coordinates shiftedMidPoint, shiftedEndPoint;
Coordinates shiftMidVec = midVec.Rotate90();
Coordinates shiftEndVec = endVec.Rotate90();
// generate multiple spline paths
for (int i = 0; i < numPaths; i++) {
shiftedMidPoint = midPoint - shiftMidVec.Normalize((i - midPathIndex) * spacing);
shiftedEndPoint = endPoint - shiftEndVec.Normalize((i - midPathIndex) * spacing);
// generate spline path
paths[i] = GenerateBezierPath(startPoint, shiftedMidPoint, startVec, midVec);
// generate extension to spline path
Path extPath = GenerateBezierPath(shiftedMidPoint, shiftedEndPoint, midVec, endVec);
// add extension to path
paths[i].Add((BezierPathSegment)extPath[0]);
}
// evaluate paths and select safest path
selectedPathIndex = EvaluatePaths(paths, midPathIndex, out pathsRisk, out pathsRiskDist, out pathsSepDist, out pathsCost);
// project further if current spline path has risk
if (pathsRisk[selectedPathIndex] != 0) {
if (projectionDist == targetLaneUpperBoundDist + TahoeParams.RL)
break;
projectionDist = Math.Min(projectionDist + TahoeParams.VL / 2, targetLaneUpperBoundDist + TahoeParams.RL);
}
} while (pathsRisk[selectedPathIndex] != 0 && projectionDist <= targetLaneUpperBoundDist + TahoeParams.RL);
// check if path without risk was found
if (pathsRisk[selectedPathIndex] == 0)
return paths[selectedPathIndex];
else
return null;
}
/// <summary>
/// Get the points along a path and their tangents and distances
/// </summary>
/// <param name="path">path to find points on</param>
/// <param name="startPoint">point on path to start from</param>
/// <param name="lookaheadDist">distance to lookahead for points</param>
/// <param name="stepDist">step distance between points on path</param>
/// <param name="pathPoints">return points on path</param>
/// <param name="pathPointTangents">returns tangents of points on paths</param>
private void GetPointsOnPath(Path path, PointOnPath startPoint, double lookaheadDist, double stepDist,
out List<Coordinates> pathPoints,
out List<Coordinates> pathPointTangents,
out List<double> pathPointDistances)
{
double tStep;
double remDist, travelDist = 0;
pathPoints = new List<Coordinates>();
pathPointTangents = new List<Coordinates>();
pathPointDistances = new List<double>();
// retrieve current segment
int segmentIndex = path.IndexOf(startPoint.segment);
BezierPathSegment segment = (BezierPathSegment)path[segmentIndex];
// determine t for start point when segment length is normalized to 1
double t = startPoint.dist / segment.Length;
// add start point and tangent
pathPoints.Add(startPoint.pt);
pathPointTangents.Add(segment.Bezier.dBdt(t));
pathPointDistances.Add(0);
// travel along path to find points until lookahead distance is reached
while (travelDist < lookaheadDist) {
// determine time step for step distance when segment length is normalized to 1
tStep = stepDist / segment.Length;
// increment t to move along path by step distance
t += tStep;
// check if reached end of current segment
if (t < 1) {
// still on current segment
// add path point
pathPoints.Add(segment.Bezier.Bt(t));
// update distance travelled along path
travelDist += stepDist;
}
else {
// reached end of current segment
t -= 1; // remaining t
// determine remaining distance on next segment
remDist = t * segment.Length;
// update distance travelled so far on current segment
travelDist += stepDist - remDist;
// increment segment index
segmentIndex += 1;
// check if reached end of path
if (segmentIndex < path.Count) {
// not at end of path yet
// retrieve next segment
segment = (BezierPathSegment)path[segmentIndex];
// determine t for remaining distance when segment length is normalized to 1
t = remDist / segment.Length;
// add path point
pathPoints.Add(segment.Bezier.Bt(t));
}
else {
// at end of path
remDist = lookaheadDist - travelDist;
// add extended path point
pathPoints.Add(segment.End + segment.Bezier.dBdt(t).Normalize(remDist));
}
// update remaining distance tarvelled along path
travelDist += remDist;
}
// add path point tangent and distance
pathPointTangents.Add(segment.Bezier.dBdt(t));
pathPointDistances.Add(travelDist);
}
}
/// <summary>
/// Startup the vehicle from a certain position, pass the next state back,
/// and initialize the lane agent if possible
/// </summary>
/// <param name="vehicleState"></param>
/// <returns></returns>
public IState Startup(VehicleState vehicleState, CarMode carMode)
{
// check car mode
if (carMode == CarMode.Run)
{
// check areas
ArbiterLane al = CoreCommon.RoadNetwork.ClosestLane(vehicleState.Front);
ArbiterZone az = CoreCommon.RoadNetwork.InZone(vehicleState.Front);
ArbiterIntersection ain = CoreCommon.RoadNetwork.InIntersection(vehicleState.Front);
ArbiterInterconnect ai = CoreCommon.RoadNetwork.ClosestInterconnect(vehicleState.Front, vehicleState.Heading);
if (az != null)
{
// special zone startup
return(new ZoneStartupState(az));
}
if (ain != null)
{
if (al != null)
{
// check lane stuff
PointOnPath lanePoint = al.PartitionPath.GetClosest(vehicleState.Front);
// get distance from front of car
double dist = lanePoint.pt.DistanceTo(vehicleState.Front);
// check dist to lane
if (dist < al.Width + 1.0)
{
// check orientation relative to lane
Coordinates laneVector = al.GetClosestPartition(vehicleState.Front).Vector().Normalize();
// get our heading
Coordinates ourHeading = vehicleState.Heading.Normalize();
// forwards or backwards
bool forwards = true;
// check dist to each other
if (laneVector.DistanceTo(ourHeading) > Math.Sqrt(2.0))
{
// not going forwards
forwards = false;
}
// stay in lane if forwards
if (forwards)
{
ArbiterOutput.Output("Starting up in lane: " + al.ToString());
return(new StayInLaneState(al, new Probability(0.7, 0.3), true, CoreCommon.CorePlanningState));
}
else
{
// Opposing lane
return(new OpposingLanesState(al, true, CoreCommon.CorePlanningState, vehicleState));
}
}
}
// startup intersection state
return(new IntersectionStartupState(ain));
}
if (al != null)
{
// get a startup chute
ArbiterLanePartition startupChute = this.GetStartupChute(vehicleState);
// check if in a startup chute
if (startupChute != null && !startupChute.IsInside(vehicleState.Front))
{
ArbiterOutput.Output("Starting up in chute: " + startupChute.ToString());
return(new StartupOffChuteState(startupChute));
}
// not in a startup chute
else
{
PointOnPath lanePoint = al.PartitionPath.GetClosest(vehicleState.Front);
// get distance from front of car
double dist = lanePoint.pt.DistanceTo(vehicleState.Front);
// check orientation relative to lane
Coordinates laneVector = al.GetClosestPartition(vehicleState.Front).Vector().Normalize();
// get our heading
Coordinates ourHeading = vehicleState.Heading.Normalize();
// forwards or backwards
bool forwards = true;
// check dist to each other
if (laneVector.DistanceTo(ourHeading) > Math.Sqrt(2.0))
{
// not going forwards
forwards = false;
}
// stay in lane if forwards
if (forwards)
{
ArbiterOutput.Output("Starting up in lane: " + al.ToString());
return(new StayInLaneState(al, new Probability(0.7, 0.3), true, CoreCommon.CorePlanningState));
}
else
{
// opposing
return(new OpposingLanesState(al, true, CoreCommon.CorePlanningState, vehicleState));
}
}
}
// fell out
ArbiterOutput.Output("Cannot find area to startup in");
return(CoreCommon.CorePlanningState);
}
else
{
return(CoreCommon.CorePlanningState);
}
}
public double GetSpeed(PointOnPath pt)
{
return((1 - pt.DistFraction) * speeds[pt.Index] + pt.DistFraction * speeds[pt.Index + 1]);
}
public Coordinates Tangent(PointOnPath pt)
{
double angle = (pt.pt - center).ArcTan;
if (ccw) {
return new Coordinates(1,0).Rotate(angle + Math.PI/2);
}
else {
return new Coordinates(1,0).Rotate(angle - Math.PI/2);
}
}
public double DistanceToGo(PointOnPath pt)
{
return DistanceToGo(pt.pt);
}
/// <summary>
/// Get polygon surrounding a path
/// </summary>
/// <param name="path"></param>
/// <param name="startPosition"></param>
/// <param name="lookaheadDist"></param>
/// <param name="stepDist"></param>
/// <param name="leftLimit"></param>
/// <param name="rightLimit"></param>
/// <returns></returns>
private Polygon GetPathPolygon(Path path, PointOnPath startPosition,
double lookaheadDist, double stepDist, double leftLimit, double rightLimit)
{
// obtain points along the path and their tangents
List<Coordinates> pathPoints, pathPointTangents;
List<double> pathPointDistances;
GetPointsOnPath(path, startPosition, lookaheadDist, stepDist,
out pathPoints, out pathPointTangents, out pathPointDistances);
// left and right boundaries of region
List<Coordinates> leftPoints = new List<Coordinates>();
List<Coordinates> rightPoints = new List<Coordinates>();
for (int i = 0; i < pathPoints.Count; i++) {
leftPoints.Add(pathPoints[i] + pathPointTangents[i].Rotate90().Normalize(leftLimit));
rightPoints.Insert(0, pathPoints[i] + pathPointTangents[i].RotateM90().Normalize(rightLimit));
}
// obtain points for sensor region
leftPoints.AddRange(rightPoints);
// prune points defining region
Coordinates prevPoint;
Coordinates nextPoint;
double prevAngle;
double nextAngle;
double diffAngle;
for (int i = 1; i < leftPoints.Count - 1; i++) {
prevPoint = leftPoints[i - 1] - leftPoints[i];
nextPoint = leftPoints[i + 1] - leftPoints[i];
prevAngle = prevPoint.ArcTan;
nextAngle = nextPoint.ArcTan;
if (prevAngle < 0) prevAngle += 2 * Math.PI;
if (nextAngle < 0) nextAngle += 2 * Math.PI;
if (prevAngle > nextAngle)
diffAngle = prevAngle - nextAngle;
else
diffAngle = nextAngle - prevAngle;
if (Math.Abs(diffAngle - Math.PI) < 5 * Math.PI / 180) {
leftPoints.RemoveAt(i);
i--;
}
}
// define sensor polygon
return new Polygon(leftPoints);
}
private double CalculateAlongTrackDist(PointOnPath pt, Coordinates loc)
{
return(pt.AlongtrackDistance(loc));
}
/// <summary>
/// Initialise vehicle and lane information
/// </summary>
private void InitialiseInformation(Coordinates position, Coordinates heading, double speed,
Path leftLanePath, Path currentLanePath, Path rightLanePath)
{
// set up vehicle information
vehiclePosition = position;
vehicleSpeed = speed;
vehicleHeading = heading.ArcTan;
// set up lane valid flags
leftLaneValid = leftLanePath != null ? true : false;
rightLaneValid = rightLanePath != null ? true : false;
// set up positions on lanes
currentLanePosition = currentLanePath.GetClosest(vehiclePosition);
leftLanePosition = leftLaneValid ? leftLanePath.GetClosest(vehiclePosition) : new PointOnPath();
rightLanePosition = rightLaneValid ? rightLanePath.GetClosest(vehiclePosition) : new PointOnPath();
// set up lane information
leftLaneWidth = leftLaneValid ? leftLanePosition.pt.DistanceTo(currentLanePosition.pt) : double.NaN;
rightLaneWidth = rightLaneValid ? rightLanePosition.pt.DistanceTo(currentLanePosition.pt) : double.NaN;
if (leftLaneValid)
currentLaneWidth = leftLaneWidth;
else if (rightLaneValid)
currentLaneWidth = rightLaneWidth;
else
currentLaneWidth = TahoeParams.T + 1.0;
}
/// <summary>
/// Advances down the path by <c>dist</c> units.
/// </summary>
/// <param name="pt">Starting point on path</param>
/// <param name="dist">
/// Distance to advance. On exit, will be the distance that could not be satisfied or 0 if the request did not go past
/// the end of the path.
/// </param>
/// <returns>
/// New advanced <c>PointOnPath</c> or <c>EndPoint</c> if past the end of the path.
/// </returns>
/// <exception cref="ArgumentOutOfRangeException">
/// Thrown when the segment specified by <c>pt</c> is not in the segments collection.
/// </exception>
/// <remarks>
/// Advances the point starting with the segment specified in <c>pt</c> and moving forward along the path. At each segment,
/// the IPathSegment.Advance method is called, then moving to the next segment until dist is exhausted.
/// </remarks>
/// <example>
/// This shows an example advancing down the first thirty meters of a path and checking if the result is past the end.
/// <code>
/// PointOnPath start = path.StartPoint;
/// double dist = 30;
/// PointOnPath thirtyMeters = path.Advance(start, ref dist);
/// if (dist > 0) {
/// Debug.WriteLine("Past the end of the path!");
/// }
/// else {
/// Debug.WriteLine("Not past the end!");
/// }
/// </code>
/// </example>
public PointOnPath AdvancePoint(PointOnPath pt, ref double dist)
{
int ind = segments.IndexOf(pt.segment);
if (ind == -1)
throw new ArgumentOutOfRangeException();
if (dist > 0)
{
while (dist > 0)
{
IPathSegment seg = pt.segment;
pt = seg.AdvancePoint(pt, ref dist);
if (dist > 0)
{
// increment the segment index
ind++;
// check if we're at the end
if (ind == segments.Count)
{
return pt;
}
else
{
// otherwise, move the PointOnPath to the beginning of the next segment
pt = segments[ind].StartPoint;
}
}
}
}
else
{
while (dist < 0)
{
IPathSegment seg = pt.segment;
pt = seg.AdvancePoint(pt, ref dist);
if (dist < 0)
{
// increment the segment index
ind--;
// check if we're at the end
if (ind == -1)
{
return pt;
}
else
{
// otherwise, move the PointOnPath to the beginning of the next segment
pt = segments[ind].EndPoint;
}
}
}
}
return pt;
}
/// <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>
/// Returns the closest segment and point that is on or past the segment specified by <c>prev</c>.
/// </summary>
/// <param name="pt">Target point</param>
/// <param name="prev">PointOnPath to start search from</param>
/// <returns>Closest segment and point past <c>prev</c></returns>
/// <exception cref="ArgumentOutOfRangeException">
/// Thrown when the segment specified by <c>prev</c> is not a member of the path's segments collection.
/// </exception>
/// <remarks>
/// Starting with the segment specified by <c>prev</c>, will search forward until a segment's closest point
/// is not it's end point, i.e. the target point is not past the end of the segment. Will return
/// <c>EndPoint</c> if none of the path segments satisify this condition, indicating that the point is past
/// the end of the path.
/// </remarks>
public PointOnPath GetClosest(Coordinates pt, PointOnPath prev)
{
int i = segments.IndexOf(prev.segment);
if (i == -1)
throw new ArgumentOutOfRangeException();
for (; i < segments.Count; i++)
{
IPathSegment seg = segments[i];
PointOnPath pop = seg.ClosestPoint(pt);
if (pop != seg.EndPoint)
{
return pop;
}
}
return EndPoint;
}
public double Curvature(PointOnPath pt)
{
return 1/radius;
}
public PointOnPath AdvancePoint(PointOnPath pt, ref double dist)
{
// assert that we're looking at the same segment
Debug.Assert(Equals(pt.segment));
if (pt.dist + dist <= 0) {
// handle the case of negative distance going before start point
dist += pt.dist;
return StartPoint;
}
else if (pt.dist + dist >= cb.ArcLength) {
// handle the case of positive distance going after end point
dist -= cb.ArcLength - pt.dist;
return EndPoint;
}
else {
// we're in the range that we can achieve
double targetDist = pt.dist + dist;
double tValue = cb.FindT(targetDist);
double actDist = cb.PartialArcLength(tValue);
dist = 0;
return new PointOnPath(this, targetDist, cb.Bt(tValue));
}
}
public Vector2 Tangent(PointOnPath pt)
{
double angle = (pt.pt - center).ArcTan;
if (ccw) {
return new Vector2(1,0).Rotate(angle + Math.PI/2);
}
else {
return new Vector2(1,0).Rotate(angle - Math.PI/2);
}
}
public double GetCurvature(PointOnPath pt)
{
if (pt.Index >= Count-1 || !pt.Valid) {
throw new ArgumentOutOfRangeException();
}
else if (pt.Index == Count-2) {
return GetCurvature(pt.Index);
}
else {
return (1-pt.DistFraction)*GetCurvature(pt.Index)+pt.DistFraction*GetCurvature(pt.Index+1);
}
}
private void GenrefVWHuniformdt(IPath path, double deltadist, double dt)
{
PointOnPath currentPointOnPath = path[0].ClosestPoint(currentPoint.ToVector2());
int numpoints = (int)((path.Length) / deltadist);
int n = 5; //number of additional points
xr = new double[numpoints + n];
yr = new double[numpoints + n];
double starttimestamp = (DateTime.Now.Ticks - 621355968000000000) / 10000000;
timestamps = new double[numpoints];
for (int i = 0; i < timestamps.Length; i++)
{
timestamps[i] = starttimestamp + i * dt;
}
for (int i = 0; i <= numpoints + 3; i++)
{
double d = deltadist * (i);
PointOnPath lookaheadpointi = path.AdvancePoint(currentPointOnPath, ref d);
xr[i] = lookaheadpointi.pt.X;
yr[i] = lookaheadpointi.pt.Y;
}
double[] xrdot = new double[xr.Length - 1];
double[] yrdot = new double[xrdot.Length];
double[] xr2dot = new double[xrdot.Length - 1];
double[] yr2dot = new double[xr2dot.Length];
double[] vr = new double[xr.Length];
double[] wr = new double[xr.Length];
double[] hr = new double[xr.Length];
for (int i = 0; i < xr.Length - 1; i++)
{
xrdot[i] = (xr[i + 1] - xr[i]) / dt;
yrdot[i] = (yr[i + 1] - yr[i]) / dt;
}
for (int i = 0; i < xrdot.Length - 1; i++)
{
xr2dot[i] = (xrdot[i + 1] - xrdot[i]) / dt;
yr2dot[i] = (yrdot[i + 1] - yrdot[i]) / dt;
}
for (int i = 0; i < xrdot.Length; i++)
{
vr[i] = Math.Sqrt(Math.Pow(xrdot[i], 2) + Math.Pow(yrdot[i], 2));
hr[i] = Math.Atan2(yrdot[i], xrdot[i]); // in radians
// unwrap headings
if (i > 0)
{
while (hr[i] - hr[i - 1] >= Math.PI)
{
hr[i] -= 2 * Math.PI;;
}
while (hr[i] - hr[i - 1] <= -Math.PI)
{
hr[i] += 2 * Math.PI;
}
}
if (i < xr2dot.Length)
{
wr[i] = ((xrdot[i] * yr2dot[i] - yrdot[i] * xr2dot[i]) / (Math.Pow(vr[i], 2))); // in radians/sec
}
}
//pad the reference vectors
for (int i = 1; i < n; i++)
{
wr[numpoints + i] = 0;
vr[numpoints + i] = 0;
hr[numpoints + i] = 0;
}
TextWriter tw = new StreamWriter("parametrization.txt");
for (int i = 0; i < timestamps.Length; i++)
{
tw.WriteLine("index timestamp x y vr wr hr");
tw.WriteLine("{0} {1} {2} {3} {4} {5} {6}", i, timestamps[i], xr[i], yr[i], vr[i], wr[i], hr[i]);
}
tw.Close();
}
public virtual PointOnPath AdvancePoint(PointOnPath pt, ref double dist)
{
Debug.Assert(Equals(pt.segment));
if (dist >= 0)
{
double d = Math.Min(DistanceToGo(pt), dist);
Vector2 tan = end - start;
tan = tan / tan.VectorLength;
dist -= d;
return new PointOnPath(this, pt.dist + d, pt.pt + tan * d);
}
else
{
double d = Math.Max(-pt.pt.DistanceTo(this.start), dist);
Vector2 tan = end - start;
tan = tan / tan.VectorLength;
dist -= d;
return new PointOnPath(this, pt.dist + d, pt.pt + tan * d);
}
}
public double DistanceToGo(PointOnPath pt)
{
// assert that we're looking at the same segment
Debug.Assert(Equals(pt.segment));
// calculate as the distance remaining
return cb.ArcLength - pt.dist;
}
public double GetSpeed(PointOnPath pt)
{
return (1-pt.DistFraction)*speeds[pt.Index]+pt.DistFraction*speeds[pt.Index+1];
}
/// <summary>
/// Calculates the distance between two points, integrated along the path.
/// </summary>
/// <param name="ptStart">Starting point</param>
/// <param name="ptEnd">Ending point</param>
/// <returns>
/// Distance between the two path points integrated along the path. If <c>startPt</c> is before
/// <c>endPt</c>, this will be positive. If <c>startPt</c> is after <c>endPt</c>, this will be negative.
/// </returns>
/// <exception cref="ArgumentOutOfRangeException">
/// Thrown when the segment specified by either <c>startPt</c> or <c>endPt</c> does not exist in the segments collection.
/// </exception>
public double DistanceBetween(PointOnPath ptStart, PointOnPath ptEnd)
{
int startIndex = segments.IndexOf(ptStart.segment);
int endIndex = segments.IndexOf(ptEnd.segment);
if (startIndex == -1 || endIndex == -1)
throw new ArgumentOutOfRangeException();
if (startIndex == endIndex)
{
return ptEnd.dist - ptStart.dist;
}
else if (startIndex < endIndex)
{
double dist = 0;
dist += ptStart.segment.DistanceToGo(ptStart);
while (++startIndex < endIndex)
{
dist += segments[startIndex].Length;
}
dist += ptEnd.dist;
return dist;
}
else
{
// startIndex > endIndex
double dist = 0;
dist -= ptStart.dist;
while (--startIndex > endIndex)
{
dist -= segments[startIndex].Length;
}
dist -= ptEnd.segment.DistanceToGo(ptEnd);
return dist;
}
}
public RobotPathMessage(int robotID, IPath path, PointOnPath goalPointOnPath, PointOnPath robotPointOnPath) : this(robotID, path)
{
this.goalPointOnPath = goalPointOnPath;
this.robotPointOnPath = robotPointOnPath;
}
public double Curvature(PointOnPath pt)
{
return 0;
}
public virtual double DistanceToGo(PointOnPath pt)
{
Debug.Assert(Equals(pt.segment));
return Length - pt.dist;
}
public double Curvature(PointOnPath pt)
{
// assert that we're looking at the same segment
Debug.Assert(Equals(pt.segment));
// short circuit for near the start/end point
if (pt.pt.ApproxEquals(cb.P0, 0.001)) {
return cb.Curvature(0);
}
else if (pt.pt.ApproxEquals(cb.P3, 0.001)) {
return cb.Curvature(1);
}
else {
// find the t-value in the general case
double tvalue = cb.FindT(pt.dist);
return cb.Curvature(tvalue);
}
}
public Vector2 Tangent(PointOnPath pt)
{
return (end - start).Normalize();
}
public Coordinates Tangent(PointOnPath pt)
{
// assert that we're looking at the same segment
Debug.Assert(Equals(pt.segment));
// short circuit for near the start/end point
if (pt.pt.ApproxEquals(cb.P0, 0.001)) {
return cb.dBdt(0).Normalize();
}
else if (pt.pt.ApproxEquals(cb.P3, 0.001)) {
return cb.dBdt(1).Normalize();
}
else {
// find the t-value in the general case
double tvalue = cb.FindT(pt.dist);
return cb.dBdt(tvalue).Normalize();
}
}
public RobotTwoWheelCommand GetCommand(double transMax, double turnMax)
{
lock (followerLock)
{
if (pathCurrentlyTracked == null)
{
Console.WriteLine("Null path tracked!");
return(new RobotTwoWheelCommand(0, 0));
}
//we are really far off the path, just get on the stupid path
//mark sucks and wants this behavior
if (pathCurrentlyTracked.Length == 0) //single waypoint case
{
goalPoint = pathCurrentlyTracked.EndPoint.pt;
startPoint = pathCurrentlyTracked.EndPoint.pt;
}
//else if (segmentCurrentlyTracked.DistanceOffPath(currentPoint.ToVector2()) > lookAheadDistParam / 2)
//{
// //Console.WriteLine("2");
// double lookaheadRef = 0;
// PointOnPath pTemp = segmentCurrentlyTracked.StartPoint;
// goalPoint = pathCurrentlyTracked.AdvancePoint(pTemp, ref lookaheadRef).pt;
//}
else
{
//Console.WriteLine("1");
//see if we can track the next segment and if so, update that...
PointOnPath currentPointOnPath = segmentCurrentlyTracked.ClosestPoint(currentPoint.ToVector2());
double lookaheadRef = lookAheadDistParam;
PointOnPath lookaheadPointOnPath = pathCurrentlyTracked.AdvancePoint(currentPointOnPath, ref lookaheadRef);
if (segmentCurrentlyTracked != lookaheadPointOnPath.segment)
{
segmentCurrentlyTracked = lookaheadPointOnPath.segment;
}
goalPoint = lookaheadPointOnPath.pt;
startPoint = currentPointOnPath.pt;
}
// Calculate errors
double errorX = (goalPoint.X - currentPoint.x);
double errorY = (goalPoint.Y - currentPoint.y);
Vector2 verr = new Vector2(errorX, errorY);
double tangentX = (goalPoint.X - startPoint.X);
double tangentY = (goalPoint.Y - startPoint.Y);
Vector2 tangent = new Vector2(tangentX, tangentY);
double errorDistance = currentPoint.ToVector2().DistanceTo(startPoint);
double currentYaw = currentPoint.yaw;
double tempCurrentYaw = currentPoint.yaw;
errorYawTangent = UnwrapAndCalculateYawError(tangentX, tangentY, ref tempCurrentYaw);
errorYaw = UnwrapAndCalculateYawError(errorX, errorY, ref currentYaw);
double tangentEquation = (goalPoint.Y - startPoint.Y) * currentPoint.x / (goalPoint.X - startPoint.Y)
- (goalPoint.Y - startPoint.Y) * goalPoint.X / (goalPoint.X - startPoint.Y) + startPoint.Y;
//Console.Clear();
//Console.WriteLine("Current yaw is: " + currentYaw);
if (tangentEquation < currentPoint.y)
{
if (Math.PI / 2 <= Math.Abs(currentYaw) && Math.Abs(currentYaw) <= Math.PI)
{
//Console.WriteLine("Above line, pointing left");
errorDistance *= -1;
}
//else Console.WriteLine("Above line, pointing right");
}
else
{
if (0 <= Math.Abs(currentYaw) && Math.Abs(currentYaw) <= Math.PI / 2)
{
//Console.WriteLine("Below line, pointing right");
errorDistance *= -1;
}
//else Console.Write("Below line, pointing left");
}
//if we are really close to last waypoint, make the robot just stop
if ((segmentCurrentlyTracked == pathCurrentlyTracked[pathCurrentlyTracked.Count - 1]) &&
(PathCurrentlyTracked.EndPoint.pt - currentPoint.ToVector2()).Length < lookAheadDistParam)
{
command = new RobotTwoWheelCommand(0, 0);
//Console.WriteLine("Acheived!");
}
else
{
//the idea here is we want to make the velocity (which is a derivative) be proportional to the error in our actual
//position
double unsignedYawErrorNormalizeToOne = Math.Abs(errorYaw) / Math.PI;
double velocityPercentage = Math.Abs((Math.PI - Math.Abs(errorYawTangent)) / Math.PI);
double commandedVelocity = (velocityPercentage < 0.5) ? 0.0 : transMax * velocityPercentage;
//double commandedHeading = 200 * errorYawTangent + 100 * errorYaw + 300 * errorDistance;
double commandedHeading = 150 * errorYawTangent + 100 * errorYaw + 200 * errorDistance;
if (Math.Abs(commandedVelocity) > transMax)
{
commandedVelocity = Math.Sign(commandedVelocity) * transMax;
}
if (Math.Abs(commandedHeading) > turnMax)
{
commandedHeading = Math.Sign(commandedHeading) * turnMax;
commandedVelocity = 0.0;
}
/*if (unsignedYawErrorNormalizeToOne > .1)
* commandedVelocity *= (1 - Math.Pow(unsignedYawErrorNormalizeToOne, velocityTurningDamingFactor));*/
command = new RobotTwoWheelCommand(commandedVelocity, commandedHeading);
//Console.WriteLine(errorYaw + ", " + errorYawTangent + ", " + errorDistance);
}
return(command);
}
}
