public LinePath LinearizeCenterLine(LinearizationOptions options)
{
LinePath transformedPath = centerlinePath;
if (options.Timestamp.IsValid)
{
RelativeTransform relTransform = Services.RelativePose.GetTransform(timestamp, options.Timestamp);
OperationalTrace.WriteVerbose("in LinearizeCenterLine, tried to find {0}->{1}, got {2}->{3}", timestamp, options.Timestamp, relTransform.OriginTimestamp, relTransform.EndTimestamp);
transformedPath = centerlinePath.Transform(relTransform);
}
LinePath.PointOnPath startPoint = transformedPath.AdvancePoint(centerlinePath.ZeroPoint, options.StartDistance);
LinePath subPath = new LinePath();;
if (options.EndDistance > options.StartDistance)
{
subPath = transformedPath.SubPath(startPoint, options.EndDistance - options.StartDistance);
}
if (subPath.Count < 2)
{
subPath.Clear();
Coordinates startPt = startPoint.Location;
subPath.Add(startPt);
subPath.Add(centerlinePath.GetSegment(startPoint.Index).UnitVector *Math.Max(options.EndDistance - options.StartDistance, 0.1) + startPt);
}
return(subPath);
}
public LinePath LinearizeCenterLine(LinearizationOptions options)
{
LinePath transformedPath = centerlinePath;
if (options.Timestamp.IsValid) {
RelativeTransform relTransform = Services.RelativePose.GetTransform(timestamp, options.Timestamp);
OperationalTrace.WriteVerbose("in LinearizeCenterLine, tried to find {0}->{1}, got {2}->{3}", timestamp, options.Timestamp, relTransform.OriginTimestamp, relTransform.EndTimestamp);
transformedPath = centerlinePath.Transform(relTransform);
}
LinePath.PointOnPath startPoint = transformedPath.AdvancePoint(centerlinePath.ZeroPoint, options.StartDistance);
LinePath subPath = new LinePath(); ;
if (options.EndDistance > options.StartDistance) {
subPath = transformedPath.SubPath(startPoint, options.EndDistance-options.StartDistance);
}
if (subPath.Count < 2) {
subPath.Clear();
Coordinates startPt = startPoint.Location;
subPath.Add(startPt);
subPath.Add(centerlinePath.GetSegment(startPoint.Index).UnitVector*Math.Max(options.EndDistance-options.StartDistance, 0.1)+startPt);
}
return subPath;
}
public LinePath LinearizeLeftBound(LinearizationOptions options)
{
LinePath bound = FindBoundary(this.width / 2 + options.LaneShiftDistance + 0.3, path);
if (options.Timestamp.IsValid)
{
RelativeTransform relTransform = Services.RelativePose.GetTransform(timestamp, options.Timestamp);
bound.TransformInPlace(relTransform);
}
if (options.EndDistance > options.StartDistance)
{
LinePath.PointOnPath startPoint = bound.AdvancePoint(bound.ZeroPoint, options.StartDistance);
return(bound.SubPath(startPoint, options.EndDistance - options.StartDistance));
}
else
{
return(new LinePath());
}
}
public LinePath LinearizeRightBound(LinearizationOptions options)
{
LinePath bound = rightBound;
if (options.Timestamp.IsValid)
{
RelativeTransform relTransform = Services.RelativePose.GetTransform(timestamp, options.Timestamp);
bound = bound.Transform(relTransform);
}
if (options.EndDistance > options.StartDistance)
{
// TODO: work off centerline path
LinePath.PointOnPath startPoint = bound.AdvancePoint(bound.ZeroPoint, options.StartDistance);
return(bound.SubPath(startPoint, options.EndDistance - options.StartDistance).ShiftLateral(options.LaneShiftDistance));
}
else
{
return(new LinePath());
}
}
public LinePath LinearizeLeftBound(LinearizationOptions options)
{
LinePath bound = leftBound;
if (options.Timestamp.IsValid) {
RelativeTransform relTransform = Services.RelativePose.GetTransform(timestamp, options.Timestamp);
bound = bound.Transform(relTransform);
}
if (options.EndDistance > options.StartDistance) {
// TODO: work off centerline path
LinePath.PointOnPath startPoint = bound.AdvancePoint(bound.ZeroPoint, options.StartDistance);
return bound.SubPath(startPoint, options.EndDistance-options.StartDistance).ShiftLateral(options.LaneShiftDistance);
}
else {
return new LinePath();
}
}
public LinePath LinearizeRightBound(LinearizationOptions options)
{
LinePath bound = FindBoundary(-this.width/2 + options.LaneShiftDistance, path);
if (options.Timestamp.IsValid) {
RelativeTransform relTransform = Services.RelativePose.GetTransform(timestamp, options.Timestamp);
bound.TransformInPlace(relTransform);
}
if (options.EndDistance > options.StartDistance) {
LinePath.PointOnPath startPoint = bound.AdvancePoint(bound.ZeroPoint, options.StartDistance);
return bound.SubPath(startPoint, options.EndDistance-options.StartDistance);
}
else {
return new LinePath();
}
}
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();
}
}
private void LinearizeLane(LinePath lane, double width, double centerStartDist, double centerEndDist, double boundStartDist, double boundEndDist, out LinePath center, out LinePath leftBound, out LinePath rightBound)
{
// create the center line model
PathLaneModel laneModel = new PathLaneModel(CarTimestamp.Invalid, new PathRoadModel.LaneEstimate(lane, width, ""));
// get the center line, left bound and right bound
LinearizationOptions laneOpts = new LinearizationOptions(centerStartDist, centerEndDist, CarTimestamp.Invalid);
center = laneModel.LinearizeCenterLine(laneOpts);
if (center == null || center.Count < 2) {
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Error, 0, "center line linearization SUCKS");
}
laneOpts = new LinearizationOptions(boundStartDist, boundEndDist, CarTimestamp.Invalid);
leftBound = laneModel.LinearizeLeftBound(laneOpts);
rightBound = laneModel.LinearizeRightBound(laneOpts);
}
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);
}
}
