public void Process(object param)
{
if (cancelled)
{
return;
}
DateTime start = HighResDateTime.Now;
OperationalVehicleState vs = Services.StateProvider.GetVehicleState();
LinePath curBasePath = basePath;
LinePath curLeftBound = leftBound;
LinePath curRightBound = rightBound;
// transform the base path to the current iteration
CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;
if (pathTime != curTimestamp)
{
RelativeTransform relTransform = Services.RelativePose.GetTransform(pathTime, curTimestamp);
curBasePath = curBasePath.Transform(relTransform);
curLeftBound = curLeftBound.Transform(relTransform);
curRightBound = curRightBound.Transform(relTransform);
}
// get the distance between the zero point and the start point
double distToStart = Coordinates.Zero.DistanceTo(curBasePath.GetPoint(curBasePath.StartPoint));
// get the sub-path between 5 and 25 meters ahead
double startDist = TahoeParams.FL;
LinePath.PointOnPath startPoint = curBasePath.AdvancePoint(curBasePath.ZeroPoint, ref startDist);
double endDist = 30;
LinePath.PointOnPath endPoint = curBasePath.AdvancePoint(startPoint, ref endDist);
if (startDist > 0)
{
// we've reached the end
Services.BehaviorManager.Execute(new HoldBrakeBehavior(), null, false);
return;
}
// get the sub-path
LinePath subPath = curBasePath.SubPath(startPoint, endPoint);
// add (0,0) as the starting point
subPath.Insert(0, new Coordinates(0, 0));
// do the same for the left, right bound
startDist = TahoeParams.FL;
endDist = 40;
startPoint = curLeftBound.AdvancePoint(curLeftBound.ZeroPoint, startDist);
endPoint = curLeftBound.AdvancePoint(startPoint, endDist);
curLeftBound = curLeftBound.SubPath(startPoint, endPoint);
startPoint = curRightBound.AdvancePoint(curRightBound.ZeroPoint, startDist);
endPoint = curRightBound.AdvancePoint(startPoint, endDist);
curRightBound = curRightBound.SubPath(startPoint, endPoint);
if (cancelled)
{
return;
}
Services.UIService.PushRelativePath(subPath, curTimestamp, "subpath");
Services.UIService.PushRelativePath(curLeftBound, curTimestamp, "left bound");
Services.UIService.PushRelativePath(curRightBound, curTimestamp, "right bound");
// run a path smoothing iteration
lock (this) {
planner = new PathPlanner();
}
////////////////////////////////////////////////////////////////////////////////////////////////////
// start of obstacle manager - hik
bool obstacleManagerEnable = true;
PathPlanner.SmoothingResult result;
if (obstacleManagerEnable == true)
{
// generate fake obstacles (for simulation testing only)
double obsSize = 10.5 / 2;
List <Coordinates> obstaclePoints = new List <Coordinates>();
List <Obstacle> obstacleClusters = new List <Obstacle>();
// fake left obstacles (for simulation only)
int totalLeftObstacles = Math.Min(0, curLeftBound.Count - 1);
for (int i = 0; i < totalLeftObstacles; i++)
{
obstaclePoints.Clear();
obstaclePoints.Add(curLeftBound[i] + new Coordinates(obsSize, obsSize));
obstaclePoints.Add(curLeftBound[i] + new Coordinates(obsSize, -obsSize));
obstaclePoints.Add(curLeftBound[i] + new Coordinates(-obsSize, -obsSize));
obstaclePoints.Add(curLeftBound[i] + new Coordinates(-obsSize, obsSize));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
}
// fake right obstacles (for simulation only)
int totalRightObstacles = Math.Min(0, curRightBound.Count - 1);
for (int i = 0; i < totalRightObstacles; i++)
{
obstaclePoints.Clear();
obstaclePoints.Add(curRightBound[i] + new Coordinates(obsSize, obsSize));
obstaclePoints.Add(curRightBound[i] + new Coordinates(obsSize, -obsSize));
obstaclePoints.Add(curRightBound[i] + new Coordinates(-obsSize, -obsSize));
obstaclePoints.Add(curRightBound[i] + new Coordinates(-obsSize, obsSize));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
}
// fake center obstacles (for simulation only)
int totalCenterObstacles = Math.Min(0, subPath.Count - 1);
for (int i = 2; i < totalCenterObstacles; i++)
{
obstaclePoints.Clear();
obstaclePoints.Add(subPath[i] + new Coordinates(obsSize, obsSize));
obstaclePoints.Add(subPath[i] + new Coordinates(obsSize, -obsSize));
obstaclePoints.Add(subPath[i] + new Coordinates(-obsSize, -obsSize));
obstaclePoints.Add(subPath[i] + new Coordinates(-obsSize, obsSize));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
}
obstaclePoints.Clear();
obstaclePoints.Add(new Coordinates(10000, 10000));
obstaclePoints.Add(new Coordinates(10000, 10001));
obstaclePoints.Add(new Coordinates(10001, 10000));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
obstaclePoints.Clear();
obstaclePoints.Add(new Coordinates(1000, 1000));
obstaclePoints.Add(new Coordinates(1000, 1001));
obstaclePoints.Add(new Coordinates(1001, 1000));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
obstaclePoints.Clear();
obstaclePoints.Add(new Coordinates(-1000, -1000));
obstaclePoints.Add(new Coordinates(-1000, -1001));
obstaclePoints.Add(new Coordinates(-1001, -1000));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
foreach (Obstacle obs in obstacleClusters)
{
obs.cspacePolygon = new Polygon(obs.obstaclePolygon.points);
}
// find obstacle path and left/right classification
LinePath obstaclePath = new LinePath();
//Boolean successFlag;
//double laneWidthAtPathEnd = 10.0;
//#warning this currently doesn't work
/*obstacleManager.ProcessObstacles(subPath, new LinePath[] { curLeftBound }, new LinePath[] { curRightBound },
* obstacleClusters, laneWidthAtPathEnd,
* out obstaclePath, out successFlag);
*/
// prepare left and right lane bounds
double laneMinSpacing = 0.1;
double laneDesiredSpacing = 0.5;
double laneAlphaS = 10000;
List <Boundary> leftBounds = new List <Boundary>();
List <Boundary> rightBounds = new List <Boundary>();
leftBounds.Add(new Boundary(curLeftBound, laneMinSpacing, laneDesiredSpacing, laneAlphaS));
rightBounds.Add(new Boundary(curRightBound, laneMinSpacing, laneDesiredSpacing, laneAlphaS));
// sort out obstacles as left and right
double obstacleMinSpacing = 0.1;
double obstacleDesiredSpacing = 1.0;
double obstacleAlphaS = 10000;
Boundary bound;
int totalObstacleClusters = obstacleClusters.Count;
for (int i = 0; i < totalObstacleClusters; i++)
{
if (obstacleClusters[i].avoidanceStatus == AvoidanceStatus.Left)
{
// obstacle cluster is to the left of obstacle path
bound = new Boundary(obstacleClusters[i].obstaclePolygon.points, obstacleMinSpacing,
obstacleDesiredSpacing, obstacleAlphaS, true);
bound.CheckFrontBumper = true;
leftBounds.Add(bound);
}
else if (obstacleClusters[i].avoidanceStatus == AvoidanceStatus.Right)
{
// obstacle cluster is to the right of obstacle path
bound = new Boundary(obstacleClusters[i].obstaclePolygon.points, obstacleMinSpacing,
obstacleDesiredSpacing, obstacleAlphaS, true);
bound.CheckFrontBumper = true;
rightBounds.Add(bound);
}
else
{
// obstacle cluster is outside grid, hence ignore obstacle cluster
}
}
Stopwatch stopwatch = new Stopwatch();
stopwatch.Start();
// PlanPath function call with obstacle path and obstacles
result = planner.PlanPath(obstaclePath, obstaclePath, leftBounds, rightBounds,
0, maxSpeed, vs.speed, null, curTimestamp, false);
stopwatch.Stop();
Console.WriteLine("============================================================");
Console.WriteLine("With ObstacleManager - Planner - Elapsed (ms): {0}", stopwatch.ElapsedMilliseconds);
Console.WriteLine("============================================================");
}
else
{
Stopwatch stopwatch = new Stopwatch();
stopwatch.Start();
// original PlanPath function call
result = planner.PlanPath(subPath, curLeftBound, curRightBound,
0, maxSpeed, vs.speed, null, curTimestamp);
stopwatch.Stop();
Console.WriteLine("============================================================");
Console.WriteLine("Without ObstacleManager - Planner - Elapsed (ms): {0}", stopwatch.ElapsedMilliseconds);
Console.WriteLine("============================================================");
}
// end of obstacle manager - hik
////////////////////////////////////////////////////////////////////////////////////////////////////
//PathPlanner.PlanningResult result = planner.PlanPath(subPath, curLeftBound, curRightBound, 0, maxSpeed, vs.speed, null, curTimestamp);
//SmoothedPath path = new SmoothedPath(pathTime);
lock (this) {
planner = null;
}
if (cancelled)
{
return;
}
if (result.result == UrbanChallenge.PathSmoothing.SmoothResult.Sucess)
{
// start tracking the path
Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetSmoothedPathVelocityCommand(result.path));
//Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetConstantSteeringConstantSpeedCommand(-.5, 2));
/*TrackingCommand cmd = new TrackingCommand(
* new FeedbackSpeedCommandGenerator(new ConstantSpeedGenerator(2, null)),
* new SinSteeringCommandGenerator(),
* true);
* Services.TrackingManager.QueueCommand(cmd);*/
// send the path's we're tracking to the UI
Services.UIService.PushRelativePath(result.path, curTimestamp, "smoothed path");
cancelled = true;
}
}
public void GenerateInterconnectPolygon(ArbiterInterconnect ai)
{
List <Coordinates> polyPoints = new List <Coordinates>();
try
{
// width
double width = 3.0;
if (ai.InitialGeneric is ArbiterWaypoint)
{
ArbiterWaypoint aw = (ArbiterWaypoint)ai.InitialGeneric;
width = width < aw.Lane.Width ? aw.Lane.Width : width;
}
if (ai.FinalGeneric is ArbiterWaypoint)
{
ArbiterWaypoint aw = (ArbiterWaypoint)ai.FinalGeneric;
width = width < aw.Lane.Width ? aw.Lane.Width : width;
}
if (ai.TurnDirection == ArbiterTurnDirection.UTurn ||
ai.TurnDirection == ArbiterTurnDirection.Straight ||
!(ai.InitialGeneric is ArbiterWaypoint) ||
!(ai.FinalGeneric is ArbiterWaypoint))
{
LinePath lp = ai.InterconnectPath.ShiftLateral(width / 2.0);
LinePath rp = ai.InterconnectPath.ShiftLateral(-width / 2.0);
polyPoints.AddRange(lp);
polyPoints.AddRange(rp);
ai.TurnPolygon = Polygon.GrahamScan(polyPoints);
if (ai.TurnDirection == ArbiterTurnDirection.UTurn)
{
List <Coordinates> updatedPts = new List <Coordinates>();
LinePath interTmp = ai.InterconnectPath.Clone();
Coordinates pathVec = ai.FinalGeneric.Position - ai.InitialGeneric.Position;
interTmp[1] = interTmp[1] + pathVec.Normalize(width / 2.0);
interTmp[0] = interTmp[0] - pathVec.Normalize(width / 2.0);
lp = interTmp.ShiftLateral(TahoeParams.VL);
rp = interTmp.ShiftLateral(-TahoeParams.VL);
updatedPts.AddRange(lp);
updatedPts.AddRange(rp);
ai.TurnPolygon = Polygon.GrahamScan(updatedPts);
}
}
else
{
// polygon points
List <Coordinates> interPoints = new List <Coordinates>();
// waypoint
ArbiterWaypoint awI = (ArbiterWaypoint)ai.InitialGeneric;
ArbiterWaypoint awF = (ArbiterWaypoint)ai.FinalGeneric;
// left and right path
LinePath leftPath = new LinePath();
LinePath rightPath = new LinePath();
// some initial points
LinePath initialPath = new LinePath(new Coordinates[] { awI.PreviousPartition.Initial.Position, awI.Position });
LinePath il = initialPath.ShiftLateral(width / 2.0);
LinePath ir = initialPath.ShiftLateral(-width / 2.0);
leftPath.Add(il[1]);
rightPath.Add(ir[1]);
// some final points
LinePath finalPath = new LinePath(new Coordinates[] { awF.Position, awF.NextPartition.Final.Position });
LinePath fl = finalPath.ShiftLateral(width / 2.0);
LinePath fr = finalPath.ShiftLateral(-width / 2.0);
leftPath.Add(fl[0]);
rightPath.Add(fr[0]);
// initial and final paths
Line iPath = new Line(awI.PreviousPartition.Initial.Position, awI.Position);
Line fPath = new Line(awF.Position, awF.NextPartition.Final.Position);
// get where the paths intersect and vector to normal path
Coordinates c;
iPath.Intersect(fPath, out c);
Coordinates vector = ai.InterconnectPath.GetClosestPoint(c).Location - c;
Coordinates center = c + vector.Normalize((vector.Length / 2.0));
// get width expansion
Coordinates iVec = awI.PreviousPartition != null?awI.PreviousPartition.Vector().Normalize(1.0) : awI.NextPartition.Vector().Normalize(1.0);
double iRot = -iVec.ArcTan;
Coordinates fVec = awF.NextPartition != null?awF.NextPartition.Vector().Normalize(1.0) : awF.PreviousPartition.Vector().Normalize(1.0);
fVec = fVec.Rotate(iRot);
double fDeg = fVec.ToDegrees();
double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI;
double centerWidth = width + width * 2.0 * Math.Abs(arcTan) / 90.0;
// get inner point (small scale)
Coordinates innerPoint = center + vector.Normalize(centerWidth / 4.0);
// get outer
Coordinates outerPoint = center - vector.Normalize(centerWidth / 2.0);
if (ai.TurnDirection == ArbiterTurnDirection.Right)
{
rightPath.Insert(1, innerPoint);
ai.InnerCoordinates = rightPath;
leftPath.Reverse();
leftPath.Insert(1, outerPoint);
Polygon p = new Polygon(leftPath.ToArray());
p.AddRange(rightPath.ToArray());
ai.TurnPolygon = p;
}
else
{
leftPath.Insert(1, innerPoint);
ai.InnerCoordinates = leftPath;
rightPath.Reverse();
rightPath.Insert(1, outerPoint);
Polygon p = new Polygon(leftPath.ToArray());
p.AddRange(rightPath.ToArray());
ai.TurnPolygon = p;
}
}
}
catch (Exception e)
{
Console.WriteLine("error generating turn polygon: " + ai.ToString());
ai.TurnPolygon = ai.DefaultPoly();
}
}
private ILaneModel GetLaneModel(LocalLaneModel laneModel, LinePath rndfPath, double rndfPathWidth, CarTimestamp rndfPathTimestamp)
{
// check the lane model probability
if (laneModel.Probability < lane_probability_reject_threshold)
{
// we're rejecting this, just return a path lane model
return(new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth));
}
// project the lane model's path into the rndf path's timestamp
RelativeTransform relTransform = Services.RelativePose.GetTransform(localRoadModel.Timestamp, rndfPathTimestamp);
LinePath laneModelPath = laneModel.LanePath.Transform(relTransform);
// iterate through the waypoints in the RNDF path and project onto the lane model
// the first one that is over the threshold, we consider the waypoint before as a potential ending point
LinePath.PointOnPath laneModelDeviationEndPoint = new LinePath.PointOnPath();
// flag indicating if any of the waypoint tests failed because of the devation was too high
bool anyDeviationTooHigh = false;
// number of waypoints accepted
int numWaypointsAccepted = 0;
// get the vehicle's position on the rndf path
LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint;
// get the vehicle's position on the lane model
LinePath.PointOnPath laneModelZeroPoint = laneModelPath.ZeroPoint;
// get the last point we want to consider on the lane model
LinePath.PointOnPath laneModelFarthestPoint = laneModelPath.AdvancePoint(laneModelZeroPoint, lane_model_max_dist);
// start walking forward through the waypoints on the rndf path
// this loop will implicitly exit when we're past the end of the lane model as the waypoints
// will stop being close to the lane model (GetClosestPoint returns the end point if we're past the
// end of the path)
for (int i = rndfZeroPoint.Index + 1; i < rndfPath.Count; i++)
{
// get the waypoint
Coordinates rndfWaypoint = rndfPath[i];
// get the closest point on the lane model
LinePath.PointOnPath laneModelClosestPoint = laneModelPath.GetClosestPoint(rndfWaypoint);
// compute the distance between the two
double deviation = rndfWaypoint.DistanceTo(laneModelClosestPoint.Location);
// if this is above the deviation threshold, leave the loop
if (deviation > lane_deviation_reject_threshold || laneModelClosestPoint > laneModelFarthestPoint)
{
// if we're at the end of the lane model path, we don't want to consider this a rejection
if (laneModelClosestPoint < laneModelFarthestPoint)
{
// mark that at least on deviation was too high
anyDeviationTooHigh = true;
}
break;
}
// increment the number of waypoint accepted
numWaypointsAccepted++;
// update the end point of where we're valid as the local road model was OK up to this point
laneModelDeviationEndPoint = laneModelClosestPoint;
}
// go through and figure out how far out the variance is within tolerance
LinePath.PointOnPath laneModelVarianceEndPoint = new LinePath.PointOnPath();
// walk forward from this point until the end of the lane mode path
for (int i = laneModelZeroPoint.Index + 1; i < laneModelPath.Count; i++)
{
// check if we're within the variance toleration
if (laneModel.LaneYVariance[i] <= y_var_reject_threshold)
{
// we are, update the point on path
laneModelVarianceEndPoint = laneModelPath.GetPointOnPath(i);
}
else
{
// we are out of tolerance, break out of the loop
break;
}
}
// now figure out everything out
// determine waypoint rejection status
WaypointRejectionResult waypointRejectionResult;
if (laneModelDeviationEndPoint.Valid)
{
// if the point is valid, that we had at least one waypoint that was ok
// check if any waypoints were rejected
if (anyDeviationTooHigh)
{
// some waypoint was ok, so we know that at least one waypoint was accepted
waypointRejectionResult = WaypointRejectionResult.SomeWaypointsAccepted;
}
else
{
// no waypoint triggered a rejection, but at least one was good
waypointRejectionResult = WaypointRejectionResult.AllWaypointsAccepted;
}
}
else
{
// the point is not valid, so we either had no waypoints or we had all rejections
if (anyDeviationTooHigh)
{
// the first waypoint was rejected, so all are rejected
waypointRejectionResult = WaypointRejectionResult.AllWaypointsRejected;
}
else
{
// the first waypoint (if any) was past the end of the lane model
waypointRejectionResult = WaypointRejectionResult.NoWaypoints;
}
}
// criteria for determining if this path is valid:
// - if some or all waypoints were accepted, than this is probably a good path
// - if some of the waypoints were accepted, we go no farther than the last waypoint that was accepted
// - if there were no waypoints, this is a potentially dangerous situation since we can't reject
// or confirm the local road model. for now, we'll assume that it is correct but this may need to change
// if we start handling intersections in this framework
// - if all waypoints were rejected, than we don't use the local road model
// - go no farther than the laneModelVarianceEndPoint, which is the last point where the y-variance of
// the lane model was in tolerance
// now build out the lane model
ILaneModel finalLaneModel;
// check if we rejected all waypoints or no lane model points satisified the variance threshold
if (waypointRejectionResult == WaypointRejectionResult.AllWaypointsRejected || !laneModelVarianceEndPoint.Valid)
{
// want to just use the path lane model
finalLaneModel = new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth);
}
else
{
// we'll use the lane model
// need to build up the center line as well as left and right bounds
// to build up the center line, use the lane model as far as we feel comfortable (limited by either variance
// or by rejections) and then use the rndf lane after that.
LinePath centerLine = new LinePath();
// figure out the max distance
// if there were no waypoints, set the laneModelDeviationEndPoint to the end of the lane model
if (waypointRejectionResult == WaypointRejectionResult.NoWaypoints)
{
laneModelDeviationEndPoint = laneModelFarthestPoint;
}
// figure out the closer of the end points
LinePath.PointOnPath laneModelEndPoint = (laneModelDeviationEndPoint < laneModelVarianceEndPoint) ? laneModelDeviationEndPoint : laneModelVarianceEndPoint;
bool endAtWaypoint = laneModelEndPoint == laneModelDeviationEndPoint;
// add the lane model to the center line
centerLine.AddRange(laneModelPath.GetSubpathEnumerator(laneModelZeroPoint, laneModelEndPoint));
// create a list to hold the width expansion values
List <double> widthValue = new List <double>();
// make the width expansion values the width of the path plus the 1-sigma values
for (int i = laneModelZeroPoint.Index; i < laneModelZeroPoint.Index + centerLine.Count; i++)
{
widthValue.Add(laneModel.Width / 2.0 + Math.Sqrt(laneModel.LaneYVariance[i]));
}
// now figure out how to add the rndf path
// get the projection of the lane model end point on the rndf path
LinePath.PointOnPath rndfPathStartPoint = rndfPath.GetClosestPoint(laneModelEndPoint.Location);
// if the closest point is past the end of rndf path, then we don't want to tack anything on
if (rndfPathStartPoint != rndfPath.EndPoint)
{
// get the last segment of the new center line
Coordinates centerLineEndSegmentVec = centerLine.EndSegment.UnitVector;
// get the last point of the new center line
Coordinates laneModelEndLoc = laneModelEndPoint.Location;
// now figure out the distance to the next waypoint
LinePath.PointOnPath rndfNextPoint = new LinePath.PointOnPath();
// figure out if we're ending at a waypoint or not
if (endAtWaypoint)
{
rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index + 1);
// if the distance from the start point to the next point is less than rndf_dist_min, then
// use the waypont after
double dist = rndfPath.DistanceBetween(rndfPathStartPoint, rndfNextPoint);
if (dist < rndf_dist_min)
{
if (rndfPathStartPoint.Index < rndfPath.Count - 2)
{
rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index + 2);
}
else if (rndfPath.DistanceBetween(rndfPathStartPoint, rndfPath.EndPoint) < rndf_dist_min)
{
rndfNextPoint = LinePath.PointOnPath.Invalid;
}
else
{
rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, rndf_dist_min * 2);
}
}
}
else
{
// track the last angle we had
double lastAngle = double.NaN;
// walk down the rndf path until we find a valid point
for (double dist = rndf_dist_min; dist <= rndf_dist_max; dist += rndf_dist_step)
{
// advance from the start point by dist
double distTemp = dist;
rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, ref distTemp);
// if the distTemp is > 0, then we're past the end of the path
if (distTemp > 0)
{
// if we're immediately past the end, we don't want to tack anything on
if (dist == rndf_dist_min)
{
rndfNextPoint = LinePath.PointOnPath.Invalid;
}
break;
}
// check the angle made by the last segment of center line and the segment
// formed between the end point of the center line and this new point
double angle = Math.Acos(centerLineEndSegmentVec.Dot((rndfNextPoint.Location - laneModelEndLoc).Normalize()));
// check if the angle satisfies the threshold or we're increasing the angle
if (Math.Abs(angle) < rndf_angle_threshold || (!double.IsNaN(lastAngle) && angle > lastAngle))
{
// break out of the loop, we're done searching
break;
}
lastAngle = angle;
}
}
// tack on the rndf starting at next point going to the end
if (rndfNextPoint.Valid)
{
LinePath subPath = rndfPath.SubPath(rndfNextPoint, rndfPath.EndPoint);
centerLine.AddRange(subPath);
// insert the lane model end point into the sub path
subPath.Insert(0, laneModelEndLoc);
// get the angles
List <Pair <int, double> > angles = subPath.GetIntersectionAngles(0, subPath.Count - 1);
// add the width of the path inflated by the angles
for (int i = 0; i < angles.Count; i++)
{
// calculate the width expansion factor
// 90 deg, 3x width
// 45 deg, 1.5x width
// 0 deg, 1x width
double widthFactor = Math.Pow(angles[i].Right / (Math.PI / 2.0), 2) * 2 + 1;
// add the width value
widthValue.Add(widthFactor * laneModel.Width / 2);
}
// add the final width
widthValue.Add(laneModel.Width / 2);
}
// set the rndf path start point to be the point we used
rndfPathStartPoint = rndfNextPoint;
}
// for now, calculate the left and right bounds the same way we do for the path lane model
// TODO: figure out if we want to do this more intelligently using knowledge of the lane model uncertainty
LinePath leftBound = centerLine.ShiftLateral(widthValue.ToArray());
// get the next shifts
for (int i = 0; i < widthValue.Count; i++)
{
widthValue[i] = -widthValue[i];
}
LinePath rightBound = centerLine.ShiftLateral(widthValue.ToArray());
// build the final lane model
finalLaneModel = new CombinedLaneModel(centerLine, leftBound, rightBound, laneModel.Width, rndfPathTimestamp);
}
SendLaneModelToUI(finalLaneModel, rndfPathTimestamp);
// output the fit result
return(finalLaneModel);
}
public override void Process(object param)
{
try {
Trace.CorrelationManager.StartLogicalOperation("ChangeLanes");
if (!base.BeginProcess())
{
return;
}
// check if we were given a parameter
if (param != null && param is ChangeLaneBehavior)
{
ChangeLaneBehavior clParam = (ChangeLaneBehavior)param;
HandleBehavior(clParam);
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "got new param -- speed {0}, dist {1}, dist timestamp {2}", clParam.SpeedCommand, clParam.MaxDist, clParam.TimeStamp);
}
// project the lane paths up to the current time
RelativeTransform relTransform = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
LinePath curStartingPath = startingLanePath.Transform(relTransform);
LinePath curEndingPath = endingLanePath.Transform(relTransform);
// get the starting and ending lane models
ILaneModel startingLaneModel, endingLaneModel;
Services.RoadModelProvider.GetLaneChangeModels(curStartingPath, startingLaneWidth, startingNumLanesLeft, startingNumLanesRight,
curEndingPath, endingLaneWidth, changeLeft, behaviorTimestamp, out startingLaneModel, out endingLaneModel);
// calculate the max speed
// TODO: make this look ahead for slowing down
settings.maxSpeed = GetMaxSpeed(endingLanePath, endingLanePath.ZeroPoint);
// get the remaining lane change distance
double remainingDist = GetRemainingLaneChangeDistance();
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "remaining distance is {0}", remainingDist);
if (cancelled)
{
return;
}
// check if we're done
if (remainingDist <= 0)
{
// create a new stay in lane behavior
int deltaLanes = changeLeft ? -1 : 1;
StayInLaneBehavior stayInLane = new StayInLaneBehavior(endingLaneID, speedCommand, null, endingLanePath, endingLaneWidth, startingNumLanesLeft + deltaLanes, startingNumLanesRight - deltaLanes);
stayInLane.TimeStamp = behaviorTimestamp.ts;
Services.BehaviorManager.Execute(stayInLane, null, false);
// send completion report
ForwardCompletionReport(new SuccessCompletionReport(typeof(ChangeLaneBehavior)));
return;
}
// calculate the planning distance
double planningDist = GetPlanningDistance();
if (planningDist < remainingDist + TahoeParams.VL)
{
planningDist = remainingDist + TahoeParams.VL;
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "planning dist {0}", planningDist);
// create a linearization options structure
LinearizationOptions laneOpts = new LinearizationOptions();
laneOpts.Timestamp = curTimestamp;
// get the center line of the target lane starting at the distance we want to enter into it
laneOpts.StartDistance = remainingDist;
laneOpts.EndDistance = planningDist;
LinePath centerLine = endingLaneModel.LinearizeCenterLine(laneOpts);
// add the final center line as a weighting
AddTargetPath(centerLine.Clone(), default_lane_alpha_w);
// pre-pend (0,0) as our position
centerLine.Insert(0, new Coordinates(0, 0));
LinePath leftBound = null;
LinePath rightBound = null;
// figure out if the lane is to the right or left of us
laneOpts.EndDistance = planningDist;
double leftEndingStartDist, rightEndingStartDist;
GetLaneBoundStartDists(curEndingPath, endingLaneWidth, out leftEndingStartDist, out rightEndingStartDist);
if (changeLeft)
{
// we're the target lane is to the left
// get the left bound of the target lane
laneOpts.StartDistance = Math.Max(leftEndingStartDist, TahoeParams.FL);
leftBound = endingLaneModel.LinearizeLeftBound(laneOpts);
}
else
{
// we're changing to the right, get the right bound of the target lane
laneOpts.StartDistance = Math.Max(rightEndingStartDist, TahoeParams.FL);
rightBound = endingLaneModel.LinearizeRightBound(laneOpts);
}
// get the other bound as the starting lane up to 5m before the remaining dist
laneOpts.StartDistance = TahoeParams.FL;
laneOpts.EndDistance = Math.Max(0, remainingDist - 5);
if (changeLeft)
{
if (laneOpts.EndDistance > 0)
{
rightBound = startingLaneModel.LinearizeRightBound(laneOpts);
}
else
{
rightBound = new LinePath();
}
}
else
{
if (laneOpts.EndDistance > 0)
{
leftBound = startingLaneModel.LinearizeLeftBound(laneOpts);
}
else
{
leftBound = new LinePath();
}
}
// append on the that bound of the target lane starting at the remaining dist
laneOpts.StartDistance = Math.Max(remainingDist, TahoeParams.FL);
laneOpts.EndDistance = planningDist;
if (changeLeft)
{
rightBound.AddRange(endingLaneModel.LinearizeRightBound(laneOpts));
}
else
{
leftBound.AddRange(endingLaneModel.LinearizeLeftBound(laneOpts));
}
// set up the planning
smootherBasePath = centerLine;
AddLeftBound(leftBound, !changeLeft);
AddRightBound(rightBound, changeLeft);
Services.UIService.PushLineList(centerLine, curTimestamp, "subpath", true);
Services.UIService.PushLineList(leftBound, curTimestamp, "left bound", true);
Services.UIService.PushLineList(rightBound, curTimestamp, "right bound", true);
if (cancelled)
{
return;
}
// set auxillary options
settings.endingHeading = centerLine.EndSegment.UnitVector.ArcTan;
// smooth and track that stuff
SmoothAndTrack(commandLabel, true);
}
finally {
Trace.CorrelationManager.StopLogicalOperation();
}
}
public void GenerateInterconnectPolygon(ArbiterInterconnect ai)
{
List<Coordinates> polyPoints = new List<Coordinates>();
try
{
// width
double width = 3.0;
if (ai.InitialGeneric is ArbiterWaypoint)
{
ArbiterWaypoint aw = (ArbiterWaypoint)ai.InitialGeneric;
width = width < aw.Lane.Width ? aw.Lane.Width : width;
}
if (ai.FinalGeneric is ArbiterWaypoint)
{
ArbiterWaypoint aw = (ArbiterWaypoint)ai.FinalGeneric;
width = width < aw.Lane.Width ? aw.Lane.Width : width;
}
if (ai.TurnDirection == ArbiterTurnDirection.UTurn ||
ai.TurnDirection == ArbiterTurnDirection.Straight ||
!(ai.InitialGeneric is ArbiterWaypoint) ||
!(ai.FinalGeneric is ArbiterWaypoint))
{
LinePath lp = ai.InterconnectPath.ShiftLateral(width / 2.0);
LinePath rp = ai.InterconnectPath.ShiftLateral(-width / 2.0);
polyPoints.AddRange(lp);
polyPoints.AddRange(rp);
ai.TurnPolygon = Polygon.GrahamScan(polyPoints);
if (ai.TurnDirection == ArbiterTurnDirection.UTurn)
{
List<Coordinates> updatedPts = new List<Coordinates>();
LinePath interTmp = ai.InterconnectPath.Clone();
Coordinates pathVec = ai.FinalGeneric.Position - ai.InitialGeneric.Position;
interTmp[1] = interTmp[1] + pathVec.Normalize(width / 2.0);
interTmp[0] = interTmp[0] - pathVec.Normalize(width / 2.0);
lp = interTmp.ShiftLateral(TahoeParams.VL);
rp = interTmp.ShiftLateral(-TahoeParams.VL);
updatedPts.AddRange(lp);
updatedPts.AddRange(rp);
ai.TurnPolygon = Polygon.GrahamScan(updatedPts);
}
}
else
{
// polygon points
List<Coordinates> interPoints = new List<Coordinates>();
// waypoint
ArbiterWaypoint awI = (ArbiterWaypoint)ai.InitialGeneric;
ArbiterWaypoint awF = (ArbiterWaypoint)ai.FinalGeneric;
// left and right path
LinePath leftPath = new LinePath();
LinePath rightPath = new LinePath();
// some initial points
LinePath initialPath = new LinePath(new Coordinates[] { awI.PreviousPartition.Initial.Position, awI.Position });
LinePath il = initialPath.ShiftLateral(width / 2.0);
LinePath ir = initialPath.ShiftLateral(-width / 2.0);
leftPath.Add(il[1]);
rightPath.Add(ir[1]);
// some final points
LinePath finalPath = new LinePath(new Coordinates[] { awF.Position, awF.NextPartition.Final.Position });
LinePath fl = finalPath.ShiftLateral(width / 2.0);
LinePath fr = finalPath.ShiftLateral(-width / 2.0);
leftPath.Add(fl[0]);
rightPath.Add(fr[0]);
// initial and final paths
Line iPath = new Line(awI.PreviousPartition.Initial.Position, awI.Position);
Line fPath = new Line(awF.Position, awF.NextPartition.Final.Position);
// get where the paths intersect and vector to normal path
Coordinates c;
iPath.Intersect(fPath, out c);
Coordinates vector = ai.InterconnectPath.GetClosestPoint(c).Location - c;
Coordinates center = c + vector.Normalize((vector.Length / 2.0));
// get width expansion
Coordinates iVec = awI.PreviousPartition != null ? awI.PreviousPartition.Vector().Normalize(1.0) : awI.NextPartition.Vector().Normalize(1.0);
double iRot = -iVec.ArcTan;
Coordinates fVec = awF.NextPartition != null ? awF.NextPartition.Vector().Normalize(1.0) : awF.PreviousPartition.Vector().Normalize(1.0);
fVec = fVec.Rotate(iRot);
double fDeg = fVec.ToDegrees();
double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI;
double centerWidth = width + width * 2.0 * Math.Abs(arcTan) / 90.0;
// get inner point (small scale)
Coordinates innerPoint = center + vector.Normalize(centerWidth / 4.0);
// get outer
Coordinates outerPoint = center - vector.Normalize(centerWidth / 2.0);
if (ai.TurnDirection == ArbiterTurnDirection.Right)
{
rightPath.Insert(1, innerPoint);
ai.InnerCoordinates = rightPath;
leftPath.Reverse();
leftPath.Insert(1, outerPoint);
Polygon p = new Polygon(leftPath.ToArray());
p.AddRange(rightPath.ToArray());
ai.TurnPolygon = p;
}
else
{
leftPath.Insert(1, innerPoint);
ai.InnerCoordinates = leftPath;
rightPath.Reverse();
rightPath.Insert(1, outerPoint);
Polygon p = new Polygon(leftPath.ToArray());
p.AddRange(rightPath.ToArray());
ai.TurnPolygon = p;
}
}
}
catch (Exception e)
{
Console.WriteLine("error generating turn polygon: " + ai.ToString());
ai.TurnPolygon = ai.DefaultPoly();
}
}
public override void Process(object param)
{
if (!base.BeginProcess())
{
return;
}
// check if we're given a param
if (param != null && param is TurnBehavior)
{
TurnBehavior turnParam = (TurnBehavior)param;
HandleTurnBehavior(turnParam);
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "turn behavior - got new param, speed command {0}", turnParam.SpeedCommand);
}
LinePath curTargetPath = targetPath.Clone();
LinePath curLeftBound = null;
if (leftBound != null)
{
curLeftBound = leftBound.Clone();
}
LinePath curRightBound = null;
if (rightBound != null)
{
curRightBound = rightBound.Clone();
}
// transform the path into the current timestamp
if (behaviorTimestamp != curTimestamp)
{
// get the relative transform
RelativeTransform relTransform = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "in turn behavior, transforming from {0}->{1}, wanted {2}->{3}", relTransform.OriginTimestamp, relTransform.EndTimestamp, behaviorTimestamp, curTimestamp);
curTargetPath.TransformInPlace(relTransform);
if (curLeftBound != null)
{
curLeftBound.TransformInPlace(relTransform);
}
if (curRightBound != null)
{
curRightBound.TransformInPlace(relTransform);
}
}
// get the distance between our current location and the start point
double distToStart = Coordinates.Zero.DistanceTo(curTargetPath[0]);
double startDist = Math.Max(0, TahoeParams.FL - distToStart);
// extract off the first 5 m of the target path
LinePath.PointOnPath endTarget = curTargetPath.AdvancePoint(curTargetPath.StartPoint, TahoeParams.VL);
curTargetPath = curTargetPath.SubPath(curTargetPath.StartPoint, endTarget);
curTargetPath = curTargetPath.RemoveZeroLengthSegments();
// adjust the left bound and right bounds starting distance
if (curLeftBound != null)
{
LinePath.PointOnPath leftBoundStart = curLeftBound.AdvancePoint(curLeftBound.StartPoint, 2);
curLeftBound = curLeftBound.SubPath(leftBoundStart, curLeftBound.EndPoint);
AddLeftBound(curLeftBound, false);
Services.UIService.PushLineList(curLeftBound, curTimestamp, "left bound", true);
}
if (curRightBound != null)
{
LinePath.PointOnPath rightBoundStart = curRightBound.AdvancePoint(curRightBound.StartPoint, 2);
curRightBound = curRightBound.SubPath(rightBoundStart, curRightBound.EndPoint);
AddRightBound(curRightBound, false);
Services.UIService.PushLineList(curRightBound, curTimestamp, "right bound", true);
}
if (cancelled)
{
return;
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "in turn behavior, dist to start: {0}", distToStart);
if (distToStart < TahoeParams.FL * 0.75)
{
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "in turn behavior, past start point of target path");
// return a completion report
ForwardCompletionReport(new SuccessCompletionReport(typeof(TurnBehavior)));
}
AddTargetPath(curTargetPath.Clone(), 0.005);
// transform the pseudo start lane to the current timestamp
AbsoluteTransformer trans = Services.StateProvider.GetAbsoluteTransformer(curTimestamp);
LinePath curStartPath = pseudoStartLane.Transform(trans);
// add the intersection pull path
LinePath intersectionPullPath = new LinePath();
double pullWeight = 0;
GetIntersectionPullPath(curStartPath, curTargetPath, intersectionPolygon, true, true, intersectionPullPath, ref pullWeight);
if (intersectionPullPath.Count > 0)
{
AddTargetPath(intersectionPullPath, pullWeight);
}
// set up planning details
// add our position to the current target path
LinePath origTargetPath = curTargetPath.Clone();
curTargetPath.Insert(0, new Coordinates(0, 0));
//curTargetPath.Insert(1, new Coordinates(1, 0));
smootherBasePath = curTargetPath;
// add the bounds
// calculate max speed
settings.maxSpeed = GetMaxSpeed(null, LinePath.PointOnPath.Invalid);
settings.Options.w_diff = 4;
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "max speed set to {0}", settings.maxSpeed);
if (cancelled)
{
return;
}
Services.UIService.PushLineList(curTargetPath, curTimestamp, "subpath", true);
// set the avoidance path to the previously planned path
// transform to the current timestamp
disablePathAngleCheck = true;
if (turnBasePath != null)
{
avoidanceBasePath = turnBasePath.Transform(trans);
avoidanceBasePath = avoidanceBasePath.SubPath(avoidanceBasePath.GetClosestPoint(Coordinates.Zero), avoidanceBasePath.EndPoint);
maxAvoidanceBasePathAdvancePoint = avoidanceBasePath.GetPointOnPath(1);
if (avoidanceBasePath.PathLength > 7.5)
{
smootherBasePath = avoidanceBasePath.SubPath(avoidanceBasePath.StartPoint, avoidanceBasePath.AdvancePoint(avoidanceBasePath.EndPoint, -7.5));
}
else
{
smootherBasePath = avoidanceBasePath.Clone();
}
disablePathAngleCheck = false;
}
// fill in auxiliary settings
if (curLeftBound != null || curRightBound != null)
{
settings.endingHeading = curTargetPath.EndSegment.UnitVector.ArcTan;
}
settings.endingPositionFixed = false;
settings.endingPositionMax = 16;
settings.endingPositionMin = -16;
if (curLeftBound != null && curLeftBound.Count > 0 && curTargetPath.Count > 0)
{
double leftWidth = curLeftBound[0].DistanceTo(origTargetPath[0]) - TahoeParams.T / 2;
settings.endingPositionMax = leftWidth;
settings.endingPositionFixed = true;
}
if (curRightBound != null && curRightBound.Count > 0 && curTargetPath.Count > 0)
{
double rightWidth = curRightBound[0].DistanceTo(origTargetPath[0]) - TahoeParams.T / 2;
settings.endingPositionFixed = true;
settings.endingPositionMin = -rightWidth;
}
//disablePathAngleCheck = true;
//useAvoidancePath = true;
// do the planning
SmoothAndTrack(commandLabel, true);
}
private void DoInitialPlan()
{
InitializePlanningSettings();
curTimestamp = Services.RelativePose.CurrentTimestamp;
vs = Services.StateProvider.GetVehicleState();
LinePath curTargetPath = targetPath.Clone();
LinePath curLeftBound = null;
if (leftBound != null)
{
curLeftBound = leftBound.Clone();
}
LinePath curRightBound = null;
if (rightBound != null)
{
curRightBound = rightBound.Clone();
}
// get the distance between our current location and the start point
double distToStart = Coordinates.Zero.DistanceTo(curTargetPath[0]);
// extract off the first 5 m of the target path
LinePath.PointOnPath endTarget = curTargetPath.AdvancePoint(curTargetPath.StartPoint, 12);
curTargetPath = curTargetPath.SubPath(curTargetPath.StartPoint, endTarget);
AddTargetPath(curTargetPath.Clone(), 0.005);
// adjust the left bound and right bounds starting distance
if (curLeftBound != null)
{
LinePath.PointOnPath leftBoundStart = curLeftBound.AdvancePoint(curLeftBound.StartPoint, 2);
curLeftBound = curLeftBound.SubPath(leftBoundStart, curLeftBound.EndPoint);
AddLeftBound(curLeftBound, false);
}
if (curRightBound != null)
{
LinePath.PointOnPath rightBoundStart = curRightBound.AdvancePoint(curRightBound.StartPoint, 2);
curRightBound = curRightBound.SubPath(rightBoundStart, curRightBound.EndPoint);
AddRightBound(curRightBound, false);
}
// add the intersection pull path
LinePath intersectionPullPath = new LinePath();
double pullWeight = 0;
AbsoluteTransformer trans = Services.StateProvider.GetAbsoluteTransformer(curTimestamp);
GetIntersectionPullPath(pseudoStartLane.Transform(trans), curTargetPath, intersectionPolygon, true, true, intersectionPullPath, ref pullWeight);
if (intersectionPullPath.Count > 0)
{
AddTargetPath(intersectionPullPath, pullWeight);
}
// set up planning details
// add our position to the current target path
curTargetPath.Insert(0, new Coordinates(0, 0));
smootherBasePath = curTargetPath;
// add the bounds
// calculate max speed
settings.maxSpeed = GetMaxSpeed(null, LinePath.PointOnPath.Invalid);
// fill in auxiliary settings
if (curLeftBound != null || curRightBound != null)
{
settings.endingHeading = curTargetPath.EndSegment.UnitVector.ArcTan;
}
disablePathAngleCheck = true;
settings.Options.w_diff = 4;
// do the planning
PlanningResult planningResult = Smooth(false);
// if the planning was a success, store the result
if (!planningResult.dynamicallyInfeasible)
{
// transform to absolute coordinates
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp);
turnBasePath = planningResult.smoothedPath.Transform(absTransform.Invert());
}
}
public override void Process(object param)
{
if (!base.BeginProcess())
{
return;
}
if (param is ZoneTravelingBehavior)
{
HandleBaseBehavior((ZoneTravelingBehavior)param);
}
extraObstacles = GetPerimeterObstacles();
if (reverseGear)
{
ProcessReverse();
return;
}
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp);
// get the vehicle relative path
LinePath relRecommendedPath = recommendedPath.Transform(absTransform);
LinePath.PointOnPath zeroPoint = relRecommendedPath.ZeroPoint;
// get the distance to the end point
double distToEnd = relRecommendedPath.DistanceBetween(zeroPoint, relRecommendedPath.EndPoint);
// get the planning distance
double planningDist = GetPlanningDistance();
planningDist = Math.Max(planningDist, 20);
planningDist -= zeroPoint.Location.Length;
if (planningDist < 2.5)
{
planningDist = 2.5;
}
if (distToEnd < planningDist)
{
// make the speed command at stop speed command
behaviorTimestamp = curTimestamp;
speedCommand = new StopAtDistSpeedCommand(distToEnd - TahoeParams.FL);
planningDist = distToEnd;
approachSpeed = recommendedSpeed.Speed;
settings.endingHeading = relRecommendedPath.EndSegment.UnitVector.ArcTan;
settings.endingPositionFixed = true;
settings.endingPositionMax = 2;
settings.endingPositionMin = -2;
}
else
{
speedCommand = new ScalarSpeedCommand(recommendedSpeed.Speed);
}
// get the distance of the path segment we care about
LinePath pathSegment = relRecommendedPath.SubPath(zeroPoint, planningDist);
double avoidanceDist = planningDist + 5;
avoidanceBasePath = relRecommendedPath.SubPath(zeroPoint, ref avoidanceDist);
if (avoidanceDist > 0)
{
avoidanceBasePath.Add(avoidanceBasePath.EndPoint.Location + avoidanceBasePath.EndSegment.Vector.Normalize(avoidanceDist));
}
// test if we should clear out of arc mode
if (arcMode)
{
if (TestNormalModeClear(relRecommendedPath, zeroPoint))
{
prevCurvature = double.NaN;
arcMode = false;
}
}
if (Math.Abs(zeroPoint.AlongtrackDistance(Coordinates.Zero)) > 1)
{
pathSegment.Insert(0, Coordinates.Zero);
}
else
{
if (pathSegment[0].DistanceTo(pathSegment[1]) < 1)
{
pathSegment.RemoveAt(0);
}
pathSegment[0] = Coordinates.Zero;
}
if (arcMode)
{
Coordinates relativeGoalPoint = relRecommendedPath.EndPoint.Location;
ArcVoteZone(relativeGoalPoint, extraObstacles);
return;
}
double pathLength = pathSegment.PathLength;
if (pathLength < 6)
{
double additionalDist = 6.25 - pathLength;
pathSegment.Add(pathSegment.EndPoint.Location + pathSegment.EndSegment.Vector.Normalize(additionalDist));
}
// determine if polygons are to the left or right of the path
for (int i = 0; i < zoneBadRegions.Length; i++)
{
Polygon poly = zoneBadRegions[i].Transform(absTransform);
int numLeft = 0;
int numRight = 0;
foreach (LineSegment ls in pathSegment.GetSegmentEnumerator())
{
for (int j = 0; j < poly.Count; j++)
{
if (ls.IsToLeft(poly[j]))
{
numLeft++;
}
else
{
numRight++;
}
}
}
if (numLeft > numRight)
{
// we'll consider this polygon on the left of the path
//additionalLeftBounds.Add(new Boundary(poly, 0.1, 0.1, 0));
}
else
{
//additionalRightBounds.Add(new Boundary(poly, 0.1, 0.1, 0));
}
}
// TODO: add zone perimeter
disablePathAngleCheck = false;
laneWidthAtPathEnd = 7;
settings.Options.w_diff = 3;
smootherBasePath = new LinePath();
smootherBasePath.Add(Coordinates.Zero);
smootherBasePath.Add(pathSegment.EndPoint.Location);
AddTargetPath(pathSegment, 0.005);
settings.maxSpeed = recommendedSpeed.Speed;
useAvoidancePath = false;
SmoothAndTrack(command_label, true);
}
protected void LinearizeStayInLane(ILaneModel laneModel, double laneDist,
double? laneStartDistMax, double? boundDistMax, double? boundStartDistMin, double? boundStartDistMax, CarTimestamp curTimestamp,
out LinePath centerLine, out LinePath leftBound, out LinePath rightBound)
{
// get the center line, left bound and right bound
LinearizationOptions laneOpts = new LinearizationOptions(0, laneDist, curTimestamp);
centerLine = laneModel.LinearizeCenterLine(laneOpts);
if (centerLine == null || centerLine.Count < 2) {
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Error, 0, "center line linearization SUCKS");
}
double leftStartDist = vs.speed*TahoeParams.actuation_delay + 1;
double rightStartDist = vs.speed*TahoeParams.actuation_delay + 1;
double offtrackDistanceBack = centerLine.ZeroPoint.OfftrackDistance(Coordinates.Zero);
bool offtrackLeftBack = offtrackDistanceBack > 0;
double additionalDistBack = 5*Math.Abs(offtrackDistanceBack)/laneModel.Width;
if (offtrackLeftBack) {
leftStartDist += additionalDistBack;
}
else {
rightStartDist += additionalDistBack;
}
// now determine how to generate the left/right bounds
// figure out the offtrack distance from the vehicle's front bumper
double offtrackDistanceFront = centerLine.ZeroPoint.OfftrackDistance(new Coordinates(TahoeParams.FL, 0));
// offtrackDistance > 0 => we're to left of path
// offtrackDistance < 0 => we're to right of path
bool offsetLeftFront = offtrackDistanceFront > 0;
double additionalDistFront = 10*Math.Abs(offtrackDistanceFront)/laneModel.Width;
if (offsetLeftFront) {
leftStartDist += additionalDistFront;
}
else {
rightStartDist += additionalDistFront;
}
if (boundStartDistMax.HasValue) {
if (leftStartDist > boundStartDistMax.Value) {
leftStartDist = boundStartDistMax.Value;
}
if (rightStartDist > boundStartDistMax.Value) {
rightStartDist = boundStartDistMax.Value;
}
}
if (boundStartDistMin.HasValue) {
if (leftStartDist < boundStartDistMin.Value) {
leftStartDist = boundStartDistMin.Value;
}
if (rightStartDist < boundStartDistMin.Value) {
rightStartDist = boundStartDistMin.Value;
}
}
double boundEndDist = laneDist + 5;
if (boundDistMax.HasValue && boundDistMax.Value < boundEndDist) {
boundEndDist = boundDistMax.Value;
}
laneOpts = new LinearizationOptions(leftStartDist, boundEndDist, curTimestamp);
leftBound = laneModel.LinearizeLeftBound(laneOpts);
laneOpts = new LinearizationOptions(rightStartDist, boundEndDist, curTimestamp);
rightBound = laneModel.LinearizeRightBound(laneOpts);
double laneStartDist = TahoeParams.FL;
if (laneStartDistMax.HasValue && laneStartDistMax.Value < laneStartDist) {
laneStartDist = laneStartDistMax.Value;
}
// remove the first FL distance of the center line
if (laneStartDist > 0) {
centerLine = centerLine.RemoveBefore(centerLine.AdvancePoint(centerLine.ZeroPoint, laneStartDist));
centerLine.Insert(0, Coordinates.Zero);
}
}
