public void GetLaneChangeModels(LinePath startingRndfPath, double startingRndfPathWidth, int startingNumLanesLeft, int startingNumLanesRight,
LinePath endingRndfPath, double endingRndfPathWidth, bool changeLeft, CarTimestamp rndfPathTimestamp,
out ILaneModel startingLaneModel, out ILaneModel endingLaneModel)
{
LocalRoadModel localRoadModel = this.localRoadModel;
// get the selected lane for the starting path
int startingSelectedLane = SelectLane(localRoadModel, startingRndfPath, startingRndfPathWidth, rndfPathTimestamp, startingNumLanesLeft, startingNumLanesRight);
// calculate the number of lanes left and right for the ending lane
int endingNumLanesLeft = startingNumLanesLeft;
int endingNumLanesRight = startingNumLanesRight;
if (changeLeft)
{
endingNumLanesLeft--;
endingNumLanesRight++;
}
else
{
endingNumLanesLeft++;
endingNumLanesRight--;
}
// get the selected lane for the ending path
int endingSelectedLane = SelectLane(localRoadModel, endingRndfPath, endingRndfPathWidth, rndfPathTimestamp, endingNumLanesLeft, endingNumLanesRight);
// check if either is invalid or the difference does not line up with the change direction
int deltaExpected = changeLeft ? 1 : -1; // starting - ending
if (startingSelectedLane < 0 || endingSelectedLane < 0 || (startingSelectedLane - endingSelectedLane) != deltaExpected)
{
// this is some invalid stuff
// we won't use either of them since we're not sure which one is truly valid
startingLaneModel = GetLaneModel(localRoadModel, -1, startingRndfPath, startingRndfPathWidth, rndfPathTimestamp);
endingLaneModel = GetLaneModel(localRoadModel, -1, endingRndfPath, endingRndfPathWidth, rndfPathTimestamp);
// mark that we did reject
didRejectLast = true;
}
else
{
// looks like the lane model selection was valid
startingLaneModel = GetLaneModel(localRoadModel, startingSelectedLane, startingRndfPath, startingRndfPathWidth, rndfPathTimestamp);
endingLaneModel = GetLaneModel(localRoadModel, endingSelectedLane, endingRndfPath, endingRndfPathWidth, rndfPathTimestamp);
// mark that we did not reject
didRejectLast = false;
}
// figure out what we want to send to the ui
// for now, just send starting lane model
SendLaneModelToUI(startingLaneModel, rndfPathTimestamp);
}
private void SendLaneModelToUI(ILaneModel laneModel, CarTimestamp ts)
{
// get the center, left and right bounds
//LinearizationOptions opts = new LinearizationOptions(0, 50, ts);
//LinePath finalCenter = laneModel.LinearizeCenterLine(opts);
//LinePath finalLeft = laneModel.LinearizeLeftBound(opts);
//LinePath finalRight = laneModel.LinearizeRightBound(opts);
//Services.UIService.PushLineList(finalCenter, ts, "lane center line", true);
//Services.UIService.PushLineList(finalLeft, ts, "lane left bound", true);
//Services.UIService.PushLineList(finalRight, ts, "lane right bound", true);
}
public void GetLaneChangeModels(LinePath startingRndfPath, double startingRndfPathWidth, int startingNumLanesLeft, int startingNumLanesRight,
LinePath endingRndfPath, double endingRndfPathWidth, bool changeLeft, CarTimestamp rndfPathTimestamp,
out ILaneModel startingLaneModel, out ILaneModel endingLaneModel)
{
LocalRoadModel localRoadModel = this.localRoadModel;
// get the selected lane for the starting path
int startingSelectedLane = SelectLane(localRoadModel, startingRndfPath, startingRndfPathWidth, rndfPathTimestamp, startingNumLanesLeft, startingNumLanesRight);
// calculate the number of lanes left and right for the ending lane
int endingNumLanesLeft = startingNumLanesLeft;
int endingNumLanesRight = startingNumLanesRight;
if (changeLeft) {
endingNumLanesLeft--;
endingNumLanesRight++;
}
else {
endingNumLanesLeft++;
endingNumLanesRight--;
}
// get the selected lane for the ending path
int endingSelectedLane = SelectLane(localRoadModel, endingRndfPath, endingRndfPathWidth, rndfPathTimestamp, endingNumLanesLeft, endingNumLanesRight);
// check if either is invalid or the difference does not line up with the change direction
int deltaExpected = changeLeft ? 1 : -1; // starting - ending
if (startingSelectedLane < 0 || endingSelectedLane < 0 || (startingSelectedLane - endingSelectedLane) != deltaExpected) {
// this is some invalid stuff
// we won't use either of them since we're not sure which one is truly valid
startingLaneModel = GetLaneModel(localRoadModel, -1, startingRndfPath, startingRndfPathWidth, rndfPathTimestamp);
endingLaneModel = GetLaneModel(localRoadModel, -1, endingRndfPath, endingRndfPathWidth, rndfPathTimestamp);
// mark that we did reject
didRejectLast = true;
}
else {
// looks like the lane model selection was valid
startingLaneModel = GetLaneModel(localRoadModel, startingSelectedLane, startingRndfPath, startingRndfPathWidth, rndfPathTimestamp);
endingLaneModel = GetLaneModel(localRoadModel, endingSelectedLane, endingRndfPath, endingRndfPathWidth, rndfPathTimestamp);
// mark that we did not reject
didRejectLast = false;
}
// figure out what we want to send to the ui
// for now, just send starting lane model
SendLaneModelToUI(startingLaneModel, rndfPathTimestamp);
}
public ILaneModel GetLaneModel(LinePath rndfPath, double rndfPathWidth, CarTimestamp rndfPathTimestamp, int numLanesLeft, int numLanesRight)
{
LocalRoadModel localRoadModel = this.localRoadModel;
int selectedLane = SelectLane(localRoadModel, rndfPath, rndfPathWidth, rndfPathTimestamp, numLanesLeft, numLanesRight);
// send the selected lane index (or rejection code) out
Services.Dataset.ItemAs <double>("selected lane").Add(selectedLane, rndfPathTimestamp);
// build the road model from the selected lane
ILaneModel finalLaneModel = GetLaneModel(localRoadModel, selectedLane, rndfPath, rndfPathWidth, rndfPathTimestamp);
// send to UI for display
SendLaneModelToUI(finalLaneModel, rndfPathTimestamp);
// return back to center
return(finalLaneModel);
}
private void SendLaneModelToUI(ILaneModel laneModel, CarTimestamp ts)
{
// get the center, left and right bounds
//LinearizationOptions opts = new LinearizationOptions(0, 50, ts);
//LinePath finalCenter = laneModel.LinearizeCenterLine(opts);
//LinePath finalLeft = laneModel.LinearizeLeftBound(opts);
//LinePath finalRight = laneModel.LinearizeRightBound(opts);
//Services.UIService.PushLineList(finalCenter, ts, "lane center line", true);
//Services.UIService.PushLineList(finalLeft, ts, "lane left bound", true);
//Services.UIService.PushLineList(finalRight, ts, "lane right bound", true);
}
public override void Process(object param)
{
// inform the base that we're beginning processing
if (!BeginProcess())
{
return;
}
// check if we were given a parameter
if (param != null && param is StayInLaneBehavior)
{
HandleBehavior((StayInLaneBehavior)param);
}
if (reverseGear)
{
ProcessReverse();
return;
}
// figure out the planning distance
double planningDistance = GetPlanningDistance();
if (sparse && planningDistance > 10)
{
planningDistance = 10;
}
double?boundStartDistMin = null;
double?boundEndDistMax = null;
if (laneID != null && Services.RoadNetwork != null)
{
ArbiterLane lane = Services.RoadNetwork.ArbiterSegments[laneID.SegmentId].Lanes[laneID];
AbsolutePose pose = Services.StateProvider.GetAbsolutePose();
ArbiterLanePartition partition = lane.GetClosestPartition(pose.xy);
LinePath partitionPath = partition.UserPartitionPath;
LinePath.PointOnPath closestPoint = partitionPath.GetClosestPoint(pose.xy);
double remainingDist = planningDistance;
double totalDist = partitionPath.DistanceBetween(closestPoint, partitionPath.EndPoint);
remainingDist -= totalDist;
if (sparse)
{
// walk ahead and determine where sparsity ends
bool nonSparseFound = false;
while (remainingDist > 0)
{
// get the next partition
partition = partition.Final.NextPartition;
if (partition == null)
{
break;
}
if (partition.Type != PartitionType.Sparse)
{
nonSparseFound = true;
break;
}
else
{
double dist = partition.UserPartitionPath.PathLength;
totalDist += dist;
remainingDist -= dist;
}
}
if (nonSparseFound)
{
boundStartDistMin = totalDist;
}
}
else
{
// determine if there is a sparse segment upcoming
bool sparseFound = false;
while (remainingDist > 0)
{
partition = partition.Final.NextPartition;
if (partition == null)
{
break;
}
if (partition.Type == PartitionType.Sparse)
{
sparseFound = true;
break;
}
else
{
double dist = partition.Length;
totalDist += dist;
remainingDist -= dist;
}
}
if (sparseFound)
{
boundEndDistMax = totalDist;
sparse = true;
}
}
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "in stay in lane, planning distance {0}", planningDistance);
// update the rndf path
RelativeTransform relTransfrom = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
LinePath curRndfPath = rndfPath.Transform(relTransfrom);
ILaneModel centerLaneModel = Services.RoadModelProvider.GetLaneModel(curRndfPath, rndfPathWidth + extraWidth, curTimestamp, numLanesLeft, numLanesRight);
double avoidanceExtra = sparse ? 5 : 7.5;
LinePath centerLine, leftBound, rightBound;
if (boundEndDistMax != null || boundStartDistMin != null)
{
LinearizeStayInLane(centerLaneModel, planningDistance + avoidanceExtra, null, boundEndDistMax, boundStartDistMin, null, curTimestamp, out centerLine, out leftBound, out rightBound);
}
else
{
LinearizeStayInLane(centerLaneModel, planningDistance + avoidanceExtra, curTimestamp, out centerLine, out leftBound, out rightBound);
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "in stay in lane, center line count: {0}, left bound count: {1}, right bound count: {2}", centerLine.Count, leftBound.Count, rightBound.Count);
// calculate time to collision of opposing obstacle if it exists
LinePath targetLine = centerLine;
double targetWeight = default_lane_alpha_w;
if (sparse)
{
targetWeight = 0.00000025;
}
else if (oncomingVehicleExists)
{
double shiftDist = -(TahoeParams.T + 1);
targetLine = centerLine.ShiftLateral(shiftDist);
targetWeight = 0.01;
}
else if (opposingLaneVehicleExists)
{
double timeToCollision = opposingLaneVehicleDist / (Math.Abs(vs.speed) + Math.Abs(opposingLaneVehicleSpeed));
Services.Dataset.ItemAs <double>("time to collision").Add(timeToCollision, curTimestamp);
if (timeToCollision < 5)
{
// shift to the right
double shiftDist = -TahoeParams.T / 2.0;
targetLine = centerLine.ShiftLateral(shiftDist);
}
}
// set up the planning
AddTargetPath(targetLine, targetWeight);
if (!sparse || boundStartDistMin != null || boundEndDistMax != null)
{
AddLeftBound(leftBound, true);
if (!oncomingVehicleExists)
{
AddRightBound(rightBound, false);
}
}
double targetDist = Math.Max(centerLine.PathLength - avoidanceExtra, planningDistance);
smootherBasePath = centerLine.SubPath(centerLine.StartPoint, targetDist);
maxSmootherBasePathAdvancePoint = smootherBasePath.EndPoint;
double extraDist = (planningDistance + avoidanceExtra) - centerLine.PathLength;
extraDist = Math.Min(extraDist, 5);
if (extraDist > 0)
{
centerLine.Add(centerLine[centerLine.Count - 1] + centerLine.EndSegment.Vector.Normalize(extraDist));
}
avoidanceBasePath = centerLine;
Services.UIService.PushLineList(centerLine, curTimestamp, "subpath", true);
Services.UIService.PushLineList(leftBound, curTimestamp, "left bound", true);
Services.UIService.PushLineList(rightBound, curTimestamp, "right bound", true);
// get the overall max speed looking forward from our current point
settings.maxSpeed = GetMaxSpeed(curRndfPath, curRndfPath.AdvancePoint(curRndfPath.ZeroPoint, vs.speed * TahoeParams.actuation_delay));
// get the max speed at the end point
settings.maxEndingSpeed = GetMaxSpeed(curRndfPath, curRndfPath.AdvancePoint(curRndfPath.ZeroPoint, planningDistance));
useAvoidancePath = false;
if (sparse)
{
// limit to 5 mph
laneWidthAtPathEnd = 20;
pathAngleCheckMax = 50;
pathAngleMax = 5 * Math.PI / 180.0;
settings.maxSpeed = Math.Min(settings.maxSpeed, 2.2352);
maxAvoidanceBasePathAdvancePoint = avoidanceBasePath.AdvancePoint(avoidanceBasePath.EndPoint, -2);
//maxSmootherBasePathAdvancePoint = smootherBasePath.AdvancePoint(smootherBasePath.EndPoint, -2);
LinePath leftEdge = RoadEdge.GetLeftEdgeLine();
LinePath rightEdge = RoadEdge.GetRightEdgeLine();
if (leftEdge != null)
{
leftLaneBounds.Add(new Boundary(leftEdge, 0.1, 1, 100, false));
}
if (rightEdge != null)
{
rightLaneBounds.Add(new Boundary(rightEdge, 0.1, 1, 100, false));
}
PlanningResult result = new PlanningResult();
ISteeringCommandGenerator commandGenerator = SparseArcVoting.SparcVote(ref prevCurvature, goalPoint);
if (commandGenerator == null)
{
// we have a block, report dynamically infeasible
result = OnDynamicallyInfeasible(null, null);
}
else
{
result.steeringCommandGenerator = commandGenerator;
result.commandLabel = commandLabel;
}
Track(result, commandLabel);
return;
}
else if (oncomingVehicleExists)
{
laneWidthAtPathEnd = 10;
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "max speed set to {0}", settings.maxSpeed);
disablePathAngleCheck = false;
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);
}
}
protected void LinearizeStayInLane(ILaneModel laneModel, double laneDist,
double? laneStartDistMax, double? boundDistMax, double? boundStartDistMax, CarTimestamp curTimestamp,
out LinePath centerLine, out LinePath leftBound, out LinePath rightBound)
{
LinearizeStayInLane(laneModel, laneDist, laneStartDistMax, boundDistMax, null, boundStartDistMax, curTimestamp, out centerLine, out leftBound, out rightBound);
}