private void ProcessReverse()
{
double planningDistance = GetPlanningDistance();
// update the rndf path
RelativeTransform relTransfrom = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
LinePath curRndfPath = rndfPath.Transform(relTransfrom);
Console.WriteLine("cur rndf path count: " + curRndfPath.Count + ", " + curRndfPath.PathLength);
Console.WriteLine("cur rndf path zero point valid: " + curRndfPath.ZeroPoint.Valid + ", loc: " + curRndfPath.ZeroPoint.Location + ", index: " + curRndfPath.ZeroPoint.Index);
Console.WriteLine("planning dist: " + planningDistance + ", stop dist: " + GetSpeedCommandStopDistance());
// get the path in reverse
double dist = -(planningDistance + TahoeParams.RL + 2);
LinePath targetPath = curRndfPath.SubPath(curRndfPath.ZeroPoint, ref dist);
if (dist < 0)
{
targetPath.Add(curRndfPath[0] - curRndfPath.GetSegment(0).Vector.Normalize(-dist));
}
Console.WriteLine("target path count: " + targetPath.Count + ", " + targetPath.PathLength);
Console.WriteLine("target path zero point valud: " + targetPath.ZeroPoint.Valid);
// generate a box by shifting the path
LinePath leftBound = targetPath.ShiftLateral(rndfPathWidth / 2.0);
LinePath rightBound = targetPath.ShiftLateral(-rndfPathWidth / 2.0);
double leftStartDist, rightStartDist;
GetLaneBoundStartDists(targetPath, rndfPathWidth, out leftStartDist, out rightStartDist);
leftBound = leftBound.RemoveBefore(leftBound.AdvancePoint(leftBound.StartPoint, leftStartDist));
rightBound = rightBound.RemoveBefore(rightBound.AdvancePoint(rightBound.StartPoint, rightStartDist));
AddTargetPath(targetPath, 0.0025);
AddLeftBound(leftBound, false);
AddRightBound(rightBound, false);
avoidanceBasePath = targetPath;
double targetDist = Math.Max(targetPath.PathLength - (TahoeParams.RL + 2), planningDistance);
smootherBasePath = targetPath.SubPath(targetPath.StartPoint, targetDist);
settings.maxSpeed = GetMaxSpeed(null, LinePath.PointOnPath.Invalid);
settings.endingPositionFixed = true;
settings.endingPositionMax = rndfPathWidth / 2.0;
settings.endingPositionMin = -rndfPathWidth / 2.0;
settings.Options.reverse = true;
Services.UIService.PushLineList(smootherBasePath, curTimestamp, "subpath", true);
Services.UIService.PushLineList(leftBound, curTimestamp, "left bound", true);
Services.UIService.PushLineList(rightBound, curTimestamp, "right bound", true);
SmoothAndTrack(commandLabel, 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);
}
}
