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);
}
protected override bool IsOffRoad()
{
// update the rndf path
RelativeTransform relTransfrom = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
LinePath curRndfPath = rndfPath.Transform(relTransfrom);
LinePath.PointOnPath closestPoint = curRndfPath.ZeroPoint;
// determine if our heading is off-set from the lane greatly
LineSegment curSegment = curRndfPath.GetSegment(closestPoint.Index);
double relativeAngle = Math.Abs(curSegment.UnitVector.ArcTan);
if (relativeAngle > 30 * Math.PI / 180.0)
{
return(true);
}
// check the offtrack distance
if (Math.Abs(closestPoint.OfftrackDistance(Coordinates.Zero)) / (rndfPathWidth / 2.0) > 1)
{
return(true);
}
return(false);
}
public DirectionAlong DirectionAlongSegment(LinePath lp)
{
// get heading of the lane path there
Coordinates pathVector = lp.GetSegment(0).UnitVector;
// get vehicle heading
Coordinates unit = new Coordinates(1, 0);
Coordinates headingVector = unit.Rotate(this.Heading.ArcTan);
// rotate vehicle heading
Coordinates relativeVehicle = headingVector.Rotate(-pathVector.ArcTan);
// get path heading
double relativeVehicleDegrees = relativeVehicle.ToDegrees() >= 180.0 ? Math.Abs(relativeVehicle.ToDegrees() - 360.0) : Math.Abs(relativeVehicle.ToDegrees());
if (relativeVehicleDegrees < 70)
{
return(DirectionAlong.Forwards);
}
else if (relativeVehicleDegrees > 70 && relativeVehicleDegrees < 110)
{
return(DirectionAlong.Perpendicular);
}
else
{
return(DirectionAlong.Reverse);
}
}
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);
}
private void ProcessReverse()
{
double planningDistance = reverseDist - Services.TrackedDistance.GetDistanceTravelled(reverseTimestamp, curTimestamp);
// update the rndf path
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp);
// get the vehicle relative path
LinePath relRecommendedPath = recommendedPath.Transform(absTransform);
LinePath targetPath;
if (relRecommendedPath.ZeroPoint.Location.Length > 10)
{
targetPath = new LinePath();
targetPath.Add(new Coordinates(0, 0));
targetPath.Add(new Coordinates(-(planningDistance + TahoeParams.RL + 2), 0));
}
else
{
// get the path in reverse
double dist = -(planningDistance + TahoeParams.RL + 2);
targetPath = relRecommendedPath.SubPath(relRecommendedPath.ZeroPoint, ref dist);
if (dist < 0)
{
targetPath.Add(relRecommendedPath[0] - relRecommendedPath.GetSegment(0).Vector.Normalize(-dist));
}
}
AddTargetPath(targetPath, 0.005);
avoidanceBasePath = targetPath;
double targetDist = Math.Max(targetPath.PathLength - (TahoeParams.RL + 2), planningDistance);
smootherBasePath = new LinePath();
smootherBasePath.Add(Coordinates.Zero);
smootherBasePath.Add(targetPath.AdvancePoint(targetPath.StartPoint, targetDist).Location);
settings.maxSpeed = recommendedSpeed.Speed;
settings.Options.reverse = true;
settings.Options.w_diff = 3;
laneWidthAtPathEnd = 5;
useAvoidancePath = false;
Services.UIService.PushLineList(smootherBasePath, curTimestamp, "subpath", true);
SmoothAndTrack(reverse_label, true);
}
private bool TestNormalModeClear(LinePath relativePath, LinePath.PointOnPath closestPoint)
{
if (arcModeTimer != null && arcModeTimer.ElapsedMilliseconds < 5000) return false;
return Math.Abs(closestPoint.OfftrackDistance(Coordinates.Zero)) < 3 && Math.Abs(relativePath.GetSegment(closestPoint.Index).UnitVector.ArcTan) < 45*Math.PI/180.0;
}
public static void SmoothAndTrack(LinePath basePath, bool useTargetPath,
IList<LinePath> leftBounds, IList<LinePath> rightBounds,
double maxSpeed, double? endingHeading, bool endingOffsetBound,
CarTimestamp curTimestamp, bool doAvoidance,
SpeedCommand speedCommand, CarTimestamp behaviorTimestamp,
string commandLabel, ref bool cancelled,
ref LinePath previousSmoothedPath, ref CarTimestamp previousSmoothedPathTimestamp, ref double? approachSpeed)
{
// if we're in listen mode, just return for now
if (OperationalBuilder.BuildMode == BuildMode.Listen) {
return;
}
PathPlanner.PlanningResult result;
double curSpeed = Services.StateProvider.GetVehicleState().speed;
LinePath targetPath = new LinePath();
double initialHeading = 0;
// get the part we just used to make a prediction
if (useTargetPath && previousSmoothedPath != null) {
//targetPath = previousSmoothedPath.Transform(Services.RelativePose.GetTransform(previousSmoothedPathTimestamp, curTimestamp));
// interpolate the path with a smoothing spline
//targetPath = targetPath.SplineInterpolate(0.05);
//Services.UIService.PushRelativePath(targetPath, curTimestamp, "prediction path2");
// calculate the point speed*dt ahead
/*double lookaheadDist = curSpeed*0.20;
if (lookaheadDist > 0.1) {
LinePath.PointOnPath pt = targetPath.AdvancePoint(targetPath.ZeroPoint, lookaheadDist);
// get the heading
initialHeading = targetPath.GetSegment(pt.Index).UnitVector.ArcTan;
// adjust the base path start point to be the predicted location
basePath[0] = pt.Location;
// get the part we just used to make a prediction
predictionPath = targetPath.SubPath(targetPath.ZeroPoint, pt);
// push to the UI
Services.UIService.PushRelativePath(predictionPath, curTimestamp, "prediction path");
//basePath[0] = new Coordinates(lookaheadDist, 0);
Services.UIService.PushRelativePath(basePath, curTimestamp, "subpath2");
Services.Dataset.ItemAs<double>("initial heading").Add(initialHeading, curTimestamp);
// calculate a piece of the sub path
//targetPath = targetPath.SubPath(targetPath.ZeroPoint, 7);
}*/
// get the tracking manager to predict stuff like whoa
AbsolutePose absPose;
OperationalVehicleState vehicleState;
Services.TrackingManager.ForwardPredict(out absPose, out vehicleState);
// insert the stuff stuff
basePath[0] = absPose.xy;
initialHeading = absPose.heading;
// start walking down the path until the angle is cool
double angle_threshold = 30*Math.PI/180.0;
double dist;
LinePath.PointOnPath newPoint = new LinePath.PointOnPath();
for (dist = 0; dist < 10; dist += 1) {
// get the point advanced from the 2nd point on the base path by dist
double distTemp = dist;
newPoint = basePath.AdvancePoint(basePath.GetPointOnPath(1), ref distTemp);
// check if we're past the end
if (distTemp > 0) {
break;
}
// check if the angle is coolness or not
double angle = Math.Acos((newPoint.Location-basePath[0]).Normalize().Dot(basePath.GetSegment(newPoint.Index).UnitVector));
if (Math.Acos(angle) < angle_threshold) {
break;
}
}
// create a new version of the base path with the stuff section removed
basePath = basePath.RemoveBetween(basePath.StartPoint, newPoint);
Services.UIService.PushRelativePath(basePath, curTimestamp, "subpath2");
// zero that stuff out
targetPath = new LinePath();
}
StaticObstacles obstacles = null;
// only do the planning is we're in a lane scenario
// otherwise, the obstacle grid will be WAY too large
if (doAvoidance && leftBounds.Count == 1 && rightBounds.Count == 1) {
// get the obstacles predicted to the current timestamp
obstacles = Services.ObstaclePipeline.GetProcessedObstacles(curTimestamp);
}
// start the planning timer
Stopwatch planningTimer = Stopwatch.StartNew();
// check if there are any obstacles
if (obstacles != null && obstacles.polygons != null && obstacles.polygons.Count > 0) {
if (cancelled) return;
// we need to do the full obstacle avoidance
// execute the obstacle manager
LinePath avoidancePath;
List<ObstacleManager.ObstacleType> obstacleSideFlags;
bool success;
Services.ObstacleManager.ProcessObstacles(basePath, leftBounds, rightBounds, obstacles.polygons,
out avoidancePath, out obstacleSideFlags, out success);
// check if we have success
if (success) {
// build the boundary lists
// start with the lanes
List<Boundary> leftSmootherBounds = new List<Boundary>();
List<Boundary> rightSmootherBounds = new List<Boundary>();
double laneMinSpacing = 0.1;
double laneDesiredSpacing = 0.1;
double laneAlphaS = 0.1;
leftSmootherBounds.Add(new Boundary(leftBounds[0], laneMinSpacing, laneDesiredSpacing, laneAlphaS));
rightSmootherBounds.Add(new Boundary(rightBounds[0], laneMinSpacing, laneDesiredSpacing, laneAlphaS));
// sort out obstacles as left and right
double obstacleMinSpacing = 0.8;
double obstacleDesiredSpacing = 0.8;
double obstacleAlphaS = 100;
int totalObstacleClusters = obstacles.polygons.Count;
for (int i = 0; i < totalObstacleClusters; i++) {
if (obstacleSideFlags[i] == ObstacleManager.ObstacleType.Left) {
Boundary bound = new Boundary(obstacles.polygons[i], obstacleMinSpacing, obstacleDesiredSpacing, obstacleAlphaS);
bound.CheckFrontBumper = true;
leftSmootherBounds.Add(bound);
}
else if (obstacleSideFlags[i] == ObstacleManager.ObstacleType.Right) {
Boundary bound = new Boundary(obstacles.polygons[i], obstacleMinSpacing, obstacleDesiredSpacing, obstacleAlphaS);
bound.CheckFrontBumper = true;
rightSmootherBounds.Add(bound);
}
}
if (cancelled) return;
// execute the smoothing
PathPlanner planner = new PathPlanner();
planner.Options.alpha_w = 0;
planner.Options.alpha_d = 10;
planner.Options.alpha_c = 10;
result = planner.PlanPath(avoidancePath, targetPath, leftSmootherBounds, rightSmootherBounds, initialHeading, maxSpeed, Services.StateProvider.GetVehicleState().speed, endingHeading, curTimestamp, endingOffsetBound);
}
else {
// mark that we did not succeed
result = new PathPlanner.PlanningResult(SmoothResult.Infeasible, null);
}
}
else {
if (cancelled) return;
// do the path smoothing
PathPlanner planner = new PathPlanner();
List<LineList> leftList = new List<LineList>();
foreach (LinePath ll in leftBounds) leftList.Add(ll);
List<LineList> rightList = new List<LineList>();
foreach (LinePath rl in rightBounds) rightList.Add(rl);
planner.Options.alpha_w = 0;
planner.Options.alpha_s = 0.1;
planner.Options.alpha_d = 10;
planner.Options.alpha_c = 10;
result = planner.PlanPath(basePath, targetPath, leftList, rightList, initialHeading, maxSpeed, Services.StateProvider.GetVehicleState().speed, endingHeading, curTimestamp, endingOffsetBound);
}
planningTimer.Stop();
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "planning took {0} ms", planningTimer.ElapsedMilliseconds);
Services.Dataset.ItemAs<bool>("route feasible").Add(result.result == SmoothResult.Sucess, LocalCarTimeProvider.LocalNow);
if (result.result == SmoothResult.Sucess) {
// insert the point (-1,0) so we make sure that the zero point during tracking is at the vehicle
//Coordinates startingVec = result.path[1].Point-result.path[0].Point;
//Coordinates insertPoint = result.path[0].Point-startingVec.Normalize();
//result.path.Insert(0, new OperationalLayer.PathPlanning.PathPoint(insertPoint, maxSpeed));
previousSmoothedPath = new LinePath(result.path);
previousSmoothedPathTimestamp = curTimestamp;
Services.UIService.PushLineList(previousSmoothedPath, curTimestamp, "smoothed path", true);
if (cancelled) return;
// we've planned out the path, now build up the command
ISpeedGenerator speedGenerator;
if (speedCommand is ScalarSpeedCommand) {
/*if (result.path.HasSpeeds) {
speedGenerator = result.path;
}
else {*/
speedGenerator = new ConstantSpeedGenerator(maxSpeed, null);
//}
}
else if (speedCommand is StopAtDistSpeedCommand) {
StopAtDistSpeedCommand stopCommand = (StopAtDistSpeedCommand)speedCommand;
IDistanceProvider distProvider = new TravelledDistanceProvider(behaviorTimestamp, stopCommand.Distance);
speedGenerator = new StopSpeedGenerator(distProvider, approachSpeed.Value);
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "stay in lane - remaining stop stop dist {0}", distProvider.GetRemainingDistance());
}
else if (speedCommand is StopAtLineSpeedCommand) {
IDistanceProvider distProvider = new StoplineDistanceProvider();
speedGenerator = new StopSpeedGenerator(distProvider, approachSpeed.Value);
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "stay in lane - remaining stop stop dist {0}", distProvider.GetRemainingDistance());
}
else if (speedCommand == null) {
throw new InvalidOperationException("Speed command is null");
}
else {
throw new InvalidOperationException("Speed command " + speedCommand.GetType().FullName + " is not supported");
}
if (cancelled) return;
// build up the command
TrackingCommand trackingCommand = new TrackingCommand(new FeedbackSpeedCommandGenerator(speedGenerator), new PathSteeringCommandGenerator(result.path), false);
trackingCommand.Label = commandLabel;
// queue it to execute
Services.TrackingManager.QueueCommand(trackingCommand);
}
}
public DirectionAlong DirectionAlongSegment(LinePath lp)
{
// get heading of the lane path there
Coordinates pathVector = lp.GetSegment(0).UnitVector;
// get vehicle heading
Coordinates unit = new Coordinates(1, 0);
Coordinates headingVector = unit.Rotate(this.Heading.ArcTan);
// rotate vehicle heading
Coordinates relativeVehicle = headingVector.Rotate(-pathVector.ArcTan);
// get path heading
double relativeVehicleDegrees = relativeVehicle.ToDegrees() >= 180.0 ? Math.Abs(relativeVehicle.ToDegrees() - 360.0) : Math.Abs(relativeVehicle.ToDegrees());
if (relativeVehicleDegrees < 70)
return DirectionAlong.Forwards;
else if (relativeVehicleDegrees > 70 && relativeVehicleDegrees < 110)
return DirectionAlong.Perpendicular;
else
return DirectionAlong.Reverse;
}
public static void SmoothAndTrack(LinePath basePath, bool useTargetPath,
IList <LinePath> leftBounds, IList <LinePath> rightBounds,
double maxSpeed, double?endingHeading, bool endingOffsetBound,
CarTimestamp curTimestamp, bool doAvoidance,
SpeedCommand speedCommand, CarTimestamp behaviorTimestamp,
string commandLabel, ref bool cancelled,
ref LinePath previousSmoothedPath, ref CarTimestamp previousSmoothedPathTimestamp, ref double?approachSpeed)
{
// if we're in listen mode, just return for now
if (OperationalBuilder.BuildMode == BuildMode.Listen)
{
return;
}
PathPlanner.PlanningResult result;
double curSpeed = Services.StateProvider.GetVehicleState().speed;
LinePath targetPath = new LinePath();
double initialHeading = 0;
// get the part we just used to make a prediction
if (useTargetPath && previousSmoothedPath != null)
{
//targetPath = previousSmoothedPath.Transform(Services.RelativePose.GetTransform(previousSmoothedPathTimestamp, curTimestamp));
// interpolate the path with a smoothing spline
//targetPath = targetPath.SplineInterpolate(0.05);
//Services.UIService.PushRelativePath(targetPath, curTimestamp, "prediction path2");
// calculate the point speed*dt ahead
/*double lookaheadDist = curSpeed*0.20;
* if (lookaheadDist > 0.1) {
* LinePath.PointOnPath pt = targetPath.AdvancePoint(targetPath.ZeroPoint, lookaheadDist);
* // get the heading
* initialHeading = targetPath.GetSegment(pt.Index).UnitVector.ArcTan;
* // adjust the base path start point to be the predicted location
* basePath[0] = pt.Location;
*
* // get the part we just used to make a prediction
* predictionPath = targetPath.SubPath(targetPath.ZeroPoint, pt);
* // push to the UI
* Services.UIService.PushRelativePath(predictionPath, curTimestamp, "prediction path");
* //basePath[0] = new Coordinates(lookaheadDist, 0);
* Services.UIService.PushRelativePath(basePath, curTimestamp, "subpath2");
* Services.Dataset.ItemAs<double>("initial heading").Add(initialHeading, curTimestamp);
* // calculate a piece of the sub path
* //targetPath = targetPath.SubPath(targetPath.ZeroPoint, 7);
* }*/
// get the tracking manager to predict stuff like whoa
AbsolutePose absPose;
OperationalVehicleState vehicleState;
Services.TrackingManager.ForwardPredict(out absPose, out vehicleState);
// insert the stuff stuff
basePath[0] = absPose.xy;
initialHeading = absPose.heading;
// start walking down the path until the angle is cool
double angle_threshold = 30 * Math.PI / 180.0;
double dist;
LinePath.PointOnPath newPoint = new LinePath.PointOnPath();
for (dist = 0; dist < 10; dist += 1)
{
// get the point advanced from the 2nd point on the base path by dist
double distTemp = dist;
newPoint = basePath.AdvancePoint(basePath.GetPointOnPath(1), ref distTemp);
// check if we're past the end
if (distTemp > 0)
{
break;
}
// check if the angle is coolness or not
double angle = Math.Acos((newPoint.Location - basePath[0]).Normalize().Dot(basePath.GetSegment(newPoint.Index).UnitVector));
if (Math.Acos(angle) < angle_threshold)
{
break;
}
}
// create a new version of the base path with the stuff section removed
basePath = basePath.RemoveBetween(basePath.StartPoint, newPoint);
Services.UIService.PushRelativePath(basePath, curTimestamp, "subpath2");
// zero that stuff out
targetPath = new LinePath();
}
StaticObstacles obstacles = null;
// only do the planning is we're in a lane scenario
// otherwise, the obstacle grid will be WAY too large
if (doAvoidance && leftBounds.Count == 1 && rightBounds.Count == 1)
{
// get the obstacles predicted to the current timestamp
obstacles = Services.ObstaclePipeline.GetProcessedObstacles(curTimestamp);
}
// start the planning timer
Stopwatch planningTimer = Stopwatch.StartNew();
// check if there are any obstacles
if (obstacles != null && obstacles.polygons != null && obstacles.polygons.Count > 0)
{
if (cancelled)
{
return;
}
// we need to do the full obstacle avoidance
// execute the obstacle manager
LinePath avoidancePath;
List <ObstacleManager.ObstacleType> obstacleSideFlags;
bool success;
Services.ObstacleManager.ProcessObstacles(basePath, leftBounds, rightBounds, obstacles.polygons,
out avoidancePath, out obstacleSideFlags, out success);
// check if we have success
if (success)
{
// build the boundary lists
// start with the lanes
List <Boundary> leftSmootherBounds = new List <Boundary>();
List <Boundary> rightSmootherBounds = new List <Boundary>();
double laneMinSpacing = 0.1;
double laneDesiredSpacing = 0.1;
double laneAlphaS = 0.1;
leftSmootherBounds.Add(new Boundary(leftBounds[0], laneMinSpacing, laneDesiredSpacing, laneAlphaS));
rightSmootherBounds.Add(new Boundary(rightBounds[0], laneMinSpacing, laneDesiredSpacing, laneAlphaS));
// sort out obstacles as left and right
double obstacleMinSpacing = 0.8;
double obstacleDesiredSpacing = 0.8;
double obstacleAlphaS = 100;
int totalObstacleClusters = obstacles.polygons.Count;
for (int i = 0; i < totalObstacleClusters; i++)
{
if (obstacleSideFlags[i] == ObstacleManager.ObstacleType.Left)
{
Boundary bound = new Boundary(obstacles.polygons[i], obstacleMinSpacing, obstacleDesiredSpacing, obstacleAlphaS);
bound.CheckFrontBumper = true;
leftSmootherBounds.Add(bound);
}
else if (obstacleSideFlags[i] == ObstacleManager.ObstacleType.Right)
{
Boundary bound = new Boundary(obstacles.polygons[i], obstacleMinSpacing, obstacleDesiredSpacing, obstacleAlphaS);
bound.CheckFrontBumper = true;
rightSmootherBounds.Add(bound);
}
}
if (cancelled)
{
return;
}
// execute the smoothing
PathPlanner planner = new PathPlanner();
planner.Options.alpha_w = 0;
planner.Options.alpha_d = 10;
planner.Options.alpha_c = 10;
result = planner.PlanPath(avoidancePath, targetPath, leftSmootherBounds, rightSmootherBounds, initialHeading, maxSpeed, Services.StateProvider.GetVehicleState().speed, endingHeading, curTimestamp, endingOffsetBound);
}
else
{
// mark that we did not succeed
result = new PathPlanner.PlanningResult(SmoothResult.Infeasible, null);
}
}
else
{
if (cancelled)
{
return;
}
// do the path smoothing
PathPlanner planner = new PathPlanner();
List <LineList> leftList = new List <LineList>();
foreach (LinePath ll in leftBounds)
{
leftList.Add(ll);
}
List <LineList> rightList = new List <LineList>();
foreach (LinePath rl in rightBounds)
{
rightList.Add(rl);
}
planner.Options.alpha_w = 0;
planner.Options.alpha_s = 0.1;
planner.Options.alpha_d = 10;
planner.Options.alpha_c = 10;
result = planner.PlanPath(basePath, targetPath, leftList, rightList, initialHeading, maxSpeed, Services.StateProvider.GetVehicleState().speed, endingHeading, curTimestamp, endingOffsetBound);
}
planningTimer.Stop();
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "planning took {0} ms", planningTimer.ElapsedMilliseconds);
Services.Dataset.ItemAs <bool>("route feasible").Add(result.result == SmoothResult.Sucess, LocalCarTimeProvider.LocalNow);
if (result.result == SmoothResult.Sucess)
{
// insert the point (-1,0) so we make sure that the zero point during tracking is at the vehicle
//Coordinates startingVec = result.path[1].Point-result.path[0].Point;
//Coordinates insertPoint = result.path[0].Point-startingVec.Normalize();
//result.path.Insert(0, new OperationalLayer.PathPlanning.PathPoint(insertPoint, maxSpeed));
previousSmoothedPath = new LinePath(result.path);
previousSmoothedPathTimestamp = curTimestamp;
Services.UIService.PushLineList(previousSmoothedPath, curTimestamp, "smoothed path", true);
if (cancelled)
{
return;
}
// we've planned out the path, now build up the command
ISpeedGenerator speedGenerator;
if (speedCommand is ScalarSpeedCommand)
{
/*if (result.path.HasSpeeds) {
* speedGenerator = result.path;
* }
* else {*/
speedGenerator = new ConstantSpeedGenerator(maxSpeed, null);
//}
}
else if (speedCommand is StopAtDistSpeedCommand)
{
StopAtDistSpeedCommand stopCommand = (StopAtDistSpeedCommand)speedCommand;
IDistanceProvider distProvider = new TravelledDistanceProvider(behaviorTimestamp, stopCommand.Distance);
speedGenerator = new StopSpeedGenerator(distProvider, approachSpeed.Value);
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "stay in lane - remaining stop stop dist {0}", distProvider.GetRemainingDistance());
}
else if (speedCommand is StopAtLineSpeedCommand)
{
IDistanceProvider distProvider = new StoplineDistanceProvider();
speedGenerator = new StopSpeedGenerator(distProvider, approachSpeed.Value);
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "stay in lane - remaining stop stop dist {0}", distProvider.GetRemainingDistance());
}
else if (speedCommand == null)
{
throw new InvalidOperationException("Speed command is null");
}
else
{
throw new InvalidOperationException("Speed command " + speedCommand.GetType().FullName + " is not supported");
}
if (cancelled)
{
return;
}
// build up the command
TrackingCommand trackingCommand = new TrackingCommand(new FeedbackSpeedCommandGenerator(speedGenerator), new PathSteeringCommandGenerator(result.path), false);
trackingCommand.Label = commandLabel;
// queue it to execute
Services.TrackingManager.QueueCommand(trackingCommand);
}
}
private LaneModelTestResult TestLane(LinePath rndfPath, CarTimestamp rndfPathTimestamp, LocalLaneModel laneModel)
{
// construct the result object to hold stuff
LaneModelTestResult result = new LaneModelTestResult();
// 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);
// get the zero point of the lane model path
LinePath.PointOnPath laneZeroPoint = laneModelPath.ZeroPoint;
// get the first waypoint on the RNDF path
LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint;
// get the heading of the rndf path at its zero point and the heading of the lane model at
// the rndf's zero point
LinePath.PointOnPath laneStartPoint = laneModelPath.GetClosestPoint(rndfZeroPoint.Location);
Coordinates laneModelHeading = laneModelPath.GetSegment(laneStartPoint.Index).UnitVector;
Coordinates rndfHeading = rndfPath.GetSegment(rndfZeroPoint.Index).UnitVector;
double angle = Math.Acos(laneModelHeading.Dot(rndfHeading));
// check if the angle is within limits for comparing offset
if (angle < 30 * Math.PI / 180.0)
{
// get the deviation between lane zero point and rndf zero point
result.rndf_deviation = rndfZeroPoint.Location.DistanceTo(laneZeroPoint.Location);
}
// now start check for how many waypoints are accepted
for (int i = rndfZeroPoint.Index + 1; i < rndfPath.Count; i++)
{
// check the distance along the rndf path
double rndfDistAlong = rndfPath.DistanceBetween(rndfZeroPoint, rndfPath.GetPointOnPath(i));
// break out if we're too far along the rndf
if (rndfDistAlong > 50)
{
break;
}
// get the waypoint
Coordinates waypoint = rndfPath[i];
// project on to lane path
LinePath.PointOnPath laneWaypoint = laneModelPath.GetClosestPoint(waypoint);
// check if we're too far along the lane path
double distAlong = laneModelPath.DistanceBetween(laneZeroPoint, laneWaypoint);
if (distAlong > lane_model_max_dist || distAlong < 0)
{
break;
}
// check if the deviation
double dist = waypoint.DistanceTo(laneWaypoint.Location);
// increment appropriate counts
if (dist < lane_deviation_reject_threshold)
{
result.forward_match_count++;
}
else
{
result.forward_rejection_count++;
}
}
// return the result
return(result);
}
private LaneModelTestResult TestLane(LinePath rndfPath, CarTimestamp rndfPathTimestamp, LocalLaneModel laneModel)
{
// construct the result object to hold stuff
LaneModelTestResult result = new LaneModelTestResult();
// 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);
// get the zero point of the lane model path
LinePath.PointOnPath laneZeroPoint = laneModelPath.ZeroPoint;
// get the first waypoint on the RNDF path
LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint;
// get the heading of the rndf path at its zero point and the heading of the lane model at
// the rndf's zero point
LinePath.PointOnPath laneStartPoint = laneModelPath.GetClosestPoint(rndfZeroPoint.Location);
Coordinates laneModelHeading = laneModelPath.GetSegment(laneStartPoint.Index).UnitVector;
Coordinates rndfHeading = rndfPath.GetSegment(rndfZeroPoint.Index).UnitVector;
double angle = Math.Acos(laneModelHeading.Dot(rndfHeading));
// check if the angle is within limits for comparing offset
if (angle < 30*Math.PI/180.0) {
// get the deviation between lane zero point and rndf zero point
result.rndf_deviation = rndfZeroPoint.Location.DistanceTo(laneZeroPoint.Location);
}
// now start check for how many waypoints are accepted
for (int i = rndfZeroPoint.Index + 1; i < rndfPath.Count; i++) {
// check the distance along the rndf path
double rndfDistAlong = rndfPath.DistanceBetween(rndfZeroPoint, rndfPath.GetPointOnPath(i));
// break out if we're too far along the rndf
if (rndfDistAlong > 50) {
break;
}
// get the waypoint
Coordinates waypoint = rndfPath[i];
// project on to lane path
LinePath.PointOnPath laneWaypoint = laneModelPath.GetClosestPoint(waypoint);
// check if we're too far along the lane path
double distAlong = laneModelPath.DistanceBetween(laneZeroPoint, laneWaypoint);
if (distAlong > lane_model_max_dist || distAlong < 0) {
break;
}
// check if the deviation
double dist = waypoint.DistanceTo(laneWaypoint.Location);
// increment appropriate counts
if (dist < lane_deviation_reject_threshold) {
result.forward_match_count++;
}
else {
result.forward_rejection_count++;
}
}
// return the result
return result;
}
private bool TestNormalModeClear(LinePath relativePath, LinePath.PointOnPath closestPoint)
{
if (arcModeTimer != null && arcModeTimer.ElapsedMilliseconds < 5000)
{
return(false);
}
return(Math.Abs(closestPoint.OfftrackDistance(Coordinates.Zero)) < 3 && Math.Abs(relativePath.GetSegment(closestPoint.Index).UnitVector.ArcTan) < 45 * Math.PI / 180.0);
}
protected void GetIntersectionPullPath(LinePath startingPath, LinePath endingPath, Polygon intersectionPolygon, bool addStartingPoint, bool addEndingPoint, LinePath targetPath, ref double pullWeight)
{
double angle = Math.Acos(startingPath.EndSegment.UnitVector.Dot(endingPath.GetSegment(0).UnitVector));
// get the centroid of the intersection
Coordinates centroid;
// check if the angle is great than an threshold
if (angle > 10*Math.PI/180.0) {
// intersect the two lines formed by the starting and ending lanes
Line startingLaneLine = new Line(startingPath[startingPath.Count-2], startingPath[startingPath.Count-1]);
Line endingLaneLine = new Line(endingPath[1], endingPath[0]);
// intersect them stuff and see if the point of intersection is between the two lines
Coordinates K;
if (!startingLaneLine.Intersect(endingLaneLine, out centroid, out K) || K.X <= 0 || K.Y <= 0)
return;
}
else {
// if there is no intersection polygon, there isn't much we can do
if (intersectionPolygon == null || intersectionPolygon.Count < 3) {
return;
}
centroid = intersectionPolygon.GetCentroid();
}
// calculate the pull weighting dependent on angle of intersection
// angle 0 -> 0 weighting
// angle 45 -> 0.00025 weighting
// angle 90 -> 0.001 weighting
pullWeight = Math.Pow(angle/(Math.PI/2), 2)*0.001;
// get the relative transform from the behavior timestamp to the current timestamp
RelativeTransform transform = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
centroid = transform.TransformPoint(centroid);
if (addStartingPoint) {
targetPath.Add(startingPath.EndPoint.Location);
}
// add the line from exit -> centroid (assuming that exit is already in the target path)
targetPath.Add(centroid);
if (addEndingPoint) {
// add the line from centroid -> entrance
targetPath.Add(endingPath[0]);
}
Services.UIService.PushLineList(targetPath, curTimestamp, "intersection path", true);
Services.Dataset.ItemAs<double>("intersection weight").Add(pullWeight, curTimestamp);
}
private LinePath ReplaceFirstPoint(LinePath path, LinePath.PointOnPath maxAdvancePoint, Coordinates pt)
{
LinePath ret;
if (!disablePathAngleCheck) {
// start walking down the path until the angle is cool
double angle_threshold = pathAngleMax;
LinePath.PointOnPath newPoint = new LinePath.PointOnPath();
for (double dist = 0; dist < pathAngleCheckMax; dist += 1) {
// get the point advanced from the 2nd point on the base path by dist
double distTemp = dist;
newPoint = path.AdvancePoint(path.GetPointOnPath(1), ref distTemp);
// check if we're past the end
if (distTemp > 0) {
break;
}
else if (maxAdvancePoint.Valid && newPoint >= maxAdvancePoint) {
newPoint = maxAdvancePoint;
break;
}
// check if the angle is coolness or not
double angle = Math.Acos((newPoint.Location-pt).Normalize().Dot(path.GetSegment(newPoint.Index).UnitVector));
if (angle < angle_threshold) {
break;
}
}
// create a new version of the base path with the stuff section removed
ret = path.RemoveBetween(path.StartPoint, newPoint);
ret[0] = pt;
}
else {
ret = path.Clone();
ret[0] = pt;
}
return ret;
}