private void HandleBehavior(UTurnBehavior cb)
{
// get the absolute transform
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(cb.TimeStamp);
this.polygonTimestamp = absTransform.Timestamp;
this.polygon = cb.Boundary.Transform(absTransform);
this.finalLane = cb.EndingLane;
this.finalSpeedCommand = cb.EndingSpeedCommand;
this.stopOnLine = cb.StopOnEndingPath;
this.stayOutPolygons = cb.StayOutPolygons;
// run constant checking if we're supposed to stop on the final line
if (stopOnLine)
{
checkMode = true;
}
else
{
checkMode = false;
}
// transform the path
LinePath.PointOnPath closestPoint = cb.EndingPath.GetClosestPoint(originalPoint);
// relativize the path
LinePath relFinalPath = cb.EndingPath.Transform(absTransform);
// get the ending orientation
finalOrientation = new LineSegment(relFinalPath[closestPoint.Index], relFinalPath[closestPoint.Index + 1]);
Services.UIService.PushLineList(cb.EndingPath, cb.TimeStamp, "original path1", false);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="safetyZoneEnd"></param>
/// <param name="safetyZoneBegin"></param>
public ArbiterSafetyZone(ArbiterLane lane, LinePath.PointOnPath safetyZoneEnd, LinePath.PointOnPath safetyZoneBegin)
{
this.safetyZoneBegin = safetyZoneBegin;
this.safetyZoneEnd = safetyZoneEnd;
this.lane = lane;
this.GenerateSafetyZone();
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="safetyZoneEnd"></param>
/// <param name="safetyZoneBegin"></param>
public ArbiterSafetyZone(ArbiterLane lane, LinePath.PointOnPath safetyZoneEnd, LinePath.PointOnPath safetyZoneBegin)
{
this.safetyZoneBegin = safetyZoneBegin;
this.safetyZoneEnd = safetyZoneEnd;
this.lane = lane;
this.GenerateSafetyZone();
}
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 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);
}
/// <summary>
/// Gets all exits downstream from a point
/// </summary>
/// <param name="position"></param>
/// <param name="way"></param>
/// <param name="exits">We are looking for exits</param>
/// <returns></returns>
private List <DownstreamPointOfInterest> Downstream(Coordinates position, ArbiterWay way, bool exits, List <ArbiterWaypoint> ignorable)
{
List <DownstreamPointOfInterest> waypoints = new List <DownstreamPointOfInterest>();
foreach (ArbiterLane al in way.Lanes.Values)
{
LinePath.PointOnPath pop = al.GetClosestPoint(position);
if (al.IsInside(position) || position.DistanceTo(al.LanePath().StartPoint.Location) < 1.0)
{
ArbiterLanePartition currentPartition = al.GetClosestPartition(position);
ArbiterWaypoint initial = currentPartition.Final;
double initialCost = position.DistanceTo(currentPartition.Final.Position) / way.Segment.SpeedLimits.MaximumSpeed;
do
{
if (((exits && currentPartition.Final.IsExit) || (!exits && currentPartition.Final.IsEntry)) && !ignorable.Contains(currentPartition.Final))
{
double timeCost = initialCost + this.TimeCostInLane(initial, currentPartition.Final);
DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
dpoi.DistanceToPoint = al.LanePath().DistanceBetween(pop, al.GetClosestPoint(currentPartition.Final.Position));
dpoi.IsExit = true;
dpoi.IsGoal = false;
dpoi.PointOfInterest = currentPartition.Final;
dpoi.TimeCostToPoint = timeCost;
waypoints.Add(dpoi);
}
currentPartition = currentPartition.Final.NextPartition;
}while(currentPartition != null);
}
}
return(waypoints);
}
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);
}
private bool CheckGeneralShape(LinePath rndfPath, CarTimestamp rndfPathTimestamp, LocalLaneModel centerLaneModel)
{
// bad newz bears
if (centerLaneModel.LanePath == null || centerLaneModel.LanePath.Count < 2)
{
return(false);
}
// project the lane model's path into the rndf path's timestamp
RelativeTransform relTransform = Services.RelativePose.GetTransform(localRoadModel.Timestamp, rndfPathTimestamp);
LinePath laneModelPath = centerLaneModel.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;
Coordinates firstWaypoint = rndfPath[rndfZeroPoint.Index + 1];
// get the projection of the first waypoint onto the lane path
LinePath.PointOnPath laneFirstWaypoint = laneModelPath.GetClosestPoint(firstWaypoint);
// get the offset vector
Coordinates offsetVector = laneFirstWaypoint.Location - firstWaypoint;
// start iterating through the waypoints forward and project them onto the rndf path
for (int i = rndfZeroPoint.Index + 1; i < rndfPath.Count; i++)
{
// get the waypoint
Coordinates waypoint = rndfPath[i];
// adjust by the first waypoint offset
waypoint += offsetVector;
// project onto the lane model
LinePath.PointOnPath laneWaypoint = laneModelPath.GetClosestPoint(waypoint);
// check the distance from the zero point on the lane model
if (laneModelPath.DistanceBetween(laneZeroPoint, laneWaypoint) > road_model_max_dist)
{
break;
}
// check the devation from the rndf
double deviation = waypoint.DistanceTo(laneWaypoint.Location);
// if the deviation is over some threshold, then we reject the model
if (deviation > road_deviation_reject_threshold)
{
return(false);
}
}
// we got this far, so this stuff is OK
return(true);
}
/// <summary>
/// checks if a pointon path is in a safety zone
/// </summary>
/// <param name="pop"></param>
/// <returns></returns>
public bool IsInSafety(LinePath.PointOnPath pop)
{
double distIn = lane.LanePath().DistanceBetween(safetyZoneBegin, pop);
if (distIn < 30 && distIn >= 0)
{
return(true);
}
else
{
return(false);
}
}
public Coordinates[] VehiclePoints(SimVehicleState svs)
{
Polygon p = VehiclePolygon(svs);
LinePath lp = new LinePath(p);
LinePath.PointOnPath c = lp.StartPoint;
List <Coordinates> cs = new List <Coordinates>();
while (!c.Equals(lp.EndPoint))
{
cs.Add(lp.GetPoint(c));
c = lp.AdvancePoint(c, 0.1);
}
return(cs.ToArray());
}
/// <summary>
/// Initial path we are coming from
/// </summary>
/// <param name="loc"></param>
/// <returns></returns>
public LinePath InitialPath(Coordinates loc)
{
if (this.Initial.Equals(this.Final))
{
// get our current position minus the subtracted dist
LinePath.PointOnPath current = this.Final.GetClosestPoint(this.Interconnect.FinalGeneric.Position);
current = this.Final.LanePath().AdvancePoint(current, MinDistFromStart + FinalSubtractedDist);
// get the default end path from this
LinePath lp = this.Final.LanePath(this.Final.LanePath().GetPoint(current), this.Interconnect.InitialGeneric.Position);
return(lp);
}
else
{
return(this.Initial.LanePath(this.Initial.WaypointList[0], (ArbiterWaypoint)this.Interconnect.InitialGeneric));
}
}
/// <summary>
/// Checks if the location is within the lane
/// </summary>
/// <param name="loc"></param>
/// <returns></returns>
public bool IsInside(Coordinates loc)
{
LinePath.PointOnPath pop = this.PartitionPath.GetClosestPoint(loc);
if (pop.Equals(this.PartitionPath.StartPoint))
{
return(false);
}
else if (pop.Equals(this.PartitionPath.EndPoint))
{
return(false);
}
else
{
return(true);
}
}
public bool RelativelyInside(Coordinates c)
{
LinePath.PointOnPath pop = this.LanePath().GetClosestPoint(c);
// check distance
if (this.IsInside(c) && pop.Location.DistanceTo(c) < this.Width * 2.5)
{
return(true);
}
else if (pop.Location.DistanceTo(c) < TahoeParams.VL * 1.5)
{
return(true);
}
else
{
return(false);
}
}
public bool RelativelyInside(Coordinates c)
{
LinePath.PointOnPath pop = this.LanePath().GetClosestPoint(c);
// check distance
if (!pop.Equals(this.LanePath().EndPoint) && !pop.Equals(this.LanePath().StartPoint) &&
pop.Location.DistanceTo(c) < TahoeParams.VL * 2.0)
{
return(true);
}
else if (pop.Location.DistanceTo(c) < TahoeParams.VL * 1.5)
{
return(true);
}
else
{
return(false);
}
}
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 HitTestResult HitTest(Coordinates loc, float tol)
{
if (line == null)
{
return(HitTestResult.NoHit);
}
LinePath.PointOnPath pt = line.GetClosestPoint(loc);
if (!pt.Valid)
{
return(HitTestResult.NoHit);
}
Coordinates closestPoint = pt.Location;
if (closestPoint.DistanceTo(loc) < tol)
{
return(new HitTestResult(this, true, closestPoint, null));
}
return(HitTestResult.NoHit);
}
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());
}
}
/// <summary>
/// Distance to another connect
/// </summary>
/// <param name="icaw"></param>
/// <returns></returns>
public double DistanceTo(IConnectAreaWaypoints icaw)
{
LinePath.PointOnPath current = this.PartitionPath.StartPoint;
double inc = 1.0;
double dist = 0;
double minDist = double.MaxValue;
while (dist == 0)
{
double tmpDist = icaw.DistanceTo(this.PartitionPath.GetPoint(current));
if (tmpDist < minDist)
{
minDist = tmpDist;
}
dist = inc;
current = PartitionPath.AdvancePoint(current, ref dist);
}
return(minDist);
}
public static void CalculateMaxPathSpeed(LinePath path, LinePath.PointOnPath startPoint, LinePath.PointOnPath endPoint, ref double maxSpeed)
{
// get the angles
List <Pair <int, double> > angles = path.GetIntersectionAngles(startPoint.Index, endPoint.Index);
foreach (Pair <int, double> angleValue in angles)
{
// calculate the desired speed to take the point
double dist = path.DistanceBetween(startPoint, path.GetPointOnPath(angleValue.Left));
// calculate the desired speed at the intersection
double desiredSpeed = 1.5 / (angleValue.Right / (Math.PI / 2));
// limit the desired speed to 2.5
desiredSpeed = Math.Max(2.5, desiredSpeed);
// calculate the speed we would be allowed to go right now
double desiredMaxSpeed = Math.Sqrt(desiredSpeed * desiredSpeed + 2 * target_decel * dist);
// check if the desired max speed is lower
if (desiredMaxSpeed < maxSpeed)
{
maxSpeed = desiredMaxSpeed;
}
}
}
public bool IsInsideClose(Coordinates c)
{
if (this.UserPartitionPath == null)
{
this.UserPartitionPath = new LinePath(new Coordinates[] { this.InitialGeneric.Position, this.FinalGeneric.Position });
}
if (this.Partition is ArbiterLanePartition)
{
ArbiterLanePartition alp = (ArbiterLanePartition)this.Partition;
LinePath.PointOnPath closest = this.UserPartitionPath.GetClosestPoint(c);
if (closest.Location.DistanceTo(c) < alp.Lane.Width)
{
if (!closest.Equals(this.UserPartitionPath.StartPoint) && !closest.Equals(this.UserPartitionPath.EndPoint))
{
return(true);
}
}
}
return(false);
}
/// <summary>
/// Gets priotiy lane determination
/// </summary>
/// <param name="ii"></param>
/// <param name="path"></param>
/// <param name="end"></param>
/// <returns></returns>
private Coordinates?LaneIntersectsPath(IntersectionInvolved ii, LinePath path, IArbiterWaypoint end)
{
ArbiterLane al = (ArbiterLane)ii.Area;
LinePath.PointOnPath current = path.StartPoint;
bool go = true;
while (go) // && !(current.Location.DistanceTo(path.EndPoint.Location) < 0.1))
{
Coordinates alClose = al.LanePath().GetClosestPoint(current.Location).Location;
double alDist = alClose.DistanceTo(current.Location);
if (alDist <= 0.05)
{
return(al.LanePath().GetClosestPoint(current.Location).Location);
}
if (current.Location.Equals(path.EndPoint.Location))
{
go = false;
}
current = path.AdvancePoint(current, 0.1);
}
/*if (ii.Exit != null)
* {
* ITraversableWaypoint laneExit = ii.Exit;
* foreach (ArbiterInterconnect tmpAi in laneExit.OutgoingConnections)
* {
* if (tmpAi.FinalGeneric.Equals(end))
* {
* return tmpAi.FinalGeneric.Position;
* }
* }
* }*/
return(null);
}
/// <summary>
/// Gets distance to next illegal place to travel in reverse
/// </summary>
public double ReverseDistanceToNextIllegal(Coordinates pos, List <ArbiterWaypoint> ignorable)
{
double minDist = double.MaxValue;
LinePath.PointOnPath posPoint = this.LanePath().GetClosestPoint(pos);
List <WaypointType> badTypes = new List <WaypointType>();
badTypes.Add(WaypointType.Stop);
badTypes.Add(WaypointType.End);
// get distance to each safety zone
foreach (ArbiterLane al in this.Way.Lanes.Values)
{
ArbiterWaypoint aw = al.GetNext(pos, badTypes, ignorable);
double d = this.LanePath().DistanceBetween(this.LanePath().GetClosestPoint(aw.Position), posPoint);
if (d < minDist)
{
minDist = d;
}
}
return(minDist);
}
/// <summary>
/// Get the vehicle direction along a lane
/// </summary>
/// <param name="lane"></param>
/// <returns></returns>
public VehicleDirectionAlongPath VehicleDirectionAlong(IFQMPlanable lane)
{
if (this.IsStopped || !this.StateMonitor.Observed.headingValid)
{
return(VehicleDirectionAlongPath.Forwards);
}
// get point on the lane path
LinePath.PointOnPath pop = lane.LanePath().GetClosestPoint(this.ClosestPosition);
// get heading of the lane path there
Coordinates pathVector = lane.LanePath().GetSegment(pop.Index).UnitVector;
// get vehicle heading
Coordinates unit = new Coordinates(1, 0);
Coordinates headingVector = unit.Rotate(this.StateMonitor.Observed.absoluteHeading);
// 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(VehicleDirectionAlongPath.Forwards);
}
else if (relativeVehicleDegrees > 70 && relativeVehicleDegrees < 110)
{
return(VehicleDirectionAlongPath.Perpendicular);
}
else
{
return(VehicleDirectionAlongPath.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);
}
}
/// <summary>
/// Generates the xySegments into segments and inputs them into the input road network
/// </summary>
/// <param name="arn"></param>
/// <returns></returns>
public ArbiterRoadNetwork GenerateSegments(ArbiterRoadNetwork arn)
{
foreach (SimpleSegment ss in segments)
{
// seg
ArbiterSegmentId asi = new ArbiterSegmentId(int.Parse(ss.Id));
ArbiterSegment asg = new ArbiterSegment(asi);
arn.ArbiterSegments.Add(asi, asg);
asg.RoadNetwork = arn;
asg.SpeedLimits = new ArbiterSpeedLimit();
asg.SpeedLimits.MaximumSpeed = 13.4112; // 30mph max speed
// way1
ArbiterWayId awi1 = new ArbiterWayId(1, asi);
ArbiterWay aw1 = new ArbiterWay(awi1);
aw1.Segment = asg;
asg.Ways.Add(awi1, aw1);
asg.Way1 = aw1;
// way2
ArbiterWayId awi2 = new ArbiterWayId(2, asi);
ArbiterWay aw2 = new ArbiterWay(awi2);
aw2.Segment = asg;
asg.Ways.Add(awi2, aw2);
asg.Way2 = aw2;
// make lanes
foreach (SimpleLane sl in ss.Lanes)
{
// lane
ArbiterLaneId ali;
ArbiterLane al;
// get way of lane id
if (ss.Way1Lanes.Contains(sl))
{
ali = new ArbiterLaneId(GenerationTools.GetId(sl.Id)[1], awi1);
al = new ArbiterLane(ali);
aw1.Lanes.Add(ali, al);
al.Way = aw1;
}
else
{
ali = new ArbiterLaneId(GenerationTools.GetId(sl.Id)[1], awi2);
al = new ArbiterLane(ali);
aw2.Lanes.Add(ali, al);
al.Way = aw2;
}
// add to display
arn.DisplayObjects.Add(al);
// width
al.Width = sl.LaneWidth == 0 ? TahoeParams.T * 2.0 : sl.LaneWidth * 0.3048;
if (sl.LaneWidth == 0)
{
Console.WriteLine("lane: " + ali.ToString() + " contains no lane width, setting to 4m");
}
// lane boundaries
al.BoundaryLeft = this.GenerateLaneBoundary(sl.LeftBound);
al.BoundaryRight = this.GenerateLaneBoundary(sl.RightBound);
// add lane to seg
asg.Lanes.Add(ali, al);
// waypoints
List <ArbiterWaypoint> waypointList = new List <ArbiterWaypoint>();
// generate waypoints
foreach (SimpleWaypoint sw in sl.Waypoints)
{
// waypoint
ArbiterWaypointId awi = new ArbiterWaypointId(GenerationTools.GetId(sw.ID)[2], ali);
ArbiterWaypoint aw = new ArbiterWaypoint(sw.Position, awi);
aw.Lane = al;
// stop
if (sl.Stops.Contains(sw.ID))
{
aw.IsStop = true;
}
// checkpoint
foreach (SimpleCheckpoint sc in sl.Checkpoints)
{
if (sw.ID == sc.WaypointId)
{
aw.IsCheckpoint = true;
aw.CheckpointId = int.Parse(sc.CheckpointId);
arn.Checkpoints.Add(aw.CheckpointId, aw);
}
}
// add
asg.Waypoints.Add(awi, aw);
arn.ArbiterWaypoints.Add(awi, aw);
al.Waypoints.Add(awi, aw);
waypointList.Add(aw);
arn.DisplayObjects.Add(aw);
arn.LegacyWaypointLookup.Add(sw.ID, aw);
}
al.WaypointList = waypointList;
// lane partitions
List <ArbiterLanePartition> alps = new List <ArbiterLanePartition>();
al.Partitions = alps;
// generate lane partitions
for (int i = 0; i < waypointList.Count - 1; i++)
{
// create lane partition
ArbiterLanePartitionId alpi = new ArbiterLanePartitionId(waypointList[i].WaypointId, waypointList[i + 1].WaypointId, ali);
ArbiterLanePartition alp = new ArbiterLanePartition(alpi, waypointList[i], waypointList[i + 1], asg);
alp.Lane = al;
waypointList[i].NextPartition = alp;
waypointList[i + 1].PreviousPartition = alp;
alps.Add(alp);
arn.DisplayObjects.Add(alp);
// crete initial user partition
ArbiterUserPartitionId aupi = new ArbiterUserPartitionId(alp.PartitionId, waypointList[i].WaypointId, waypointList[i + 1].WaypointId);
ArbiterUserPartition aup = new ArbiterUserPartition(aupi, alp, waypointList[i], waypointList[i + 1]);
List <ArbiterUserPartition> aups = new List <ArbiterUserPartition>();
aups.Add(aup);
alp.UserPartitions = aups;
alp.SetDefaultSparsePolygon();
arn.DisplayObjects.Add(aup);
}
// path segments of lane
List <IPathSegment> ips = new List <IPathSegment>();
List <Coordinates> pathSegments = new List <Coordinates>();
pathSegments.Add(alps[0].Initial.Position);
// loop
foreach (ArbiterLanePartition alPar in alps)
{
ips.Add(new LinePathSegment(alPar.Initial.Position, alPar.Final.Position));
// make new segment
pathSegments.Add(alPar.Final.Position);
}
// generate lane partition path
LinePath partitionPath = new LinePath(pathSegments);
al.SetLanePath(partitionPath);
al.PartitionPath = new Path(ips, CoordinateMode.AbsoluteProjected);
// safeto zones
foreach (ArbiterWaypoint aw in al.Waypoints.Values)
{
if (aw.IsStop)
{
LinePath.PointOnPath end = al.GetClosestPoint(aw.Position);
double dist = -30;
LinePath.PointOnPath begin = al.LanePath().AdvancePoint(end, ref dist);
if (dist != 0)
{
begin = al.LanePath().StartPoint;
}
ArbiterSafetyZone asz = new ArbiterSafetyZone(al, end, begin);
asz.isExit = true;
asz.Exit = aw;
al.SafetyZones.Add(asz);
arn.DisplayObjects.Add(asz);
arn.ArbiterSafetyZones.Add(asz);
}
}
}
}
return(arn);
}
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;
}
public double DistanceTo(LinePath.PointOnPath endPoint)
{
return(DistanceBetween(ZeroPoint, endPoint));
}
/// <summary>
/// Checks if a coordinate is in a safety zone
/// </summary>
/// <param name="c"></param>
/// <returns></returns>
public bool IsInSafety(Coordinates c)
{
LinePath.PointOnPath pop = lane.GetClosestPoint(c);
return(this.IsInSafety(pop));
}
public static Polygon PartitionPolygon(ArbiterLanePartition alp)
{
if (alp.Initial.PreviousPartition != null &&
alp.Final.NextPartition != null &&
alp.Length < 30.0 && alp.Length > 4.0)
{
// get partition turn direction
ArbiterTurnDirection pTD = PartitionTurnDirection(alp);
// check if angles of previous and next are such that not straight through
if (pTD != ArbiterTurnDirection.Straight)
{
// get partition poly
ArbiterInterconnect tmpAi = alp.ToInterconnect;
tmpAi.TurnDirection = pTD;
GenerateInterconnectPolygon(tmpAi);
Polygon pPoly = tmpAi.TurnPolygon;
// here is default partition polygon
LinePath alplb = alp.PartitionPath.ShiftLateral(-alp.Lane.Width / 2.0);
LinePath alprb = alp.PartitionPath.ShiftLateral(alp.Lane.Width / 2.0);
alprb.Reverse();
List <Coordinates> alpdefaultPoly = alplb;
alpdefaultPoly.AddRange(alprb);
// get full poly
pPoly.AddRange(alpdefaultPoly);
pPoly = Polygon.GrahamScan(pPoly);
return(pPoly);
}
}
else if (alp.Length >= 30)
{
Polygon pBase = GenerateSimplePartitionPolygon(alp, alp.PartitionPath, alp.Lane.Width);
if (alp.Initial.PreviousPartition != null && Math.Abs(FinalIntersectionAngle(alp.Initial.PreviousPartition)) > 15)
{
// initial portion
Coordinates i1 = alp.Initial.Position - alp.Initial.PreviousPartition.Vector().Normalize(15.0);
Coordinates i2 = alp.Initial.Position;
Coordinates i3 = i2 + alp.Vector().Normalize(15.0);
LinePath il12 = new LinePath(new Coordinates[] { i1, i2 });
LinePath il23 = new LinePath(new Coordinates[] { i2, i3 });
LinePath il13 = new LinePath(new Coordinates[] { i1, i3 });
Coordinates iCC = il13.GetClosestPoint(i2).Location;
if (GeneralToolkit.TriangleArea(i1, i2, i3) < 0)
{
il13 = il13.ShiftLateral(iCC.DistanceTo(i2) + alp.Lane.Width / 2.0);
}
else
{
il13 = il13.ShiftLateral(-iCC.DistanceTo(i2) + alp.Lane.Width / 2.0);
}
LinePath.PointOnPath iCCP = il13.GetClosestPoint(iCC);
iCCP = il13.AdvancePoint(iCCP, -10.0);
il13 = il13.SubPath(iCCP, 20.0);
Polygon iBase = GenerateSimplePolygon(il23, alp.Lane.Width);
iBase.Add(il13[1]);
Polygon iP = Polygon.GrahamScan(iBase);
pBase = PolygonToolkit.PolygonUnion(new List <Polygon>(new Polygon[] { pBase, iP }));
}
if (alp.Final.NextPartition != null && Math.Abs(FinalIntersectionAngle(alp)) > 15)
{
// initial portion
Coordinates i1 = alp.Final.Position - alp.Vector().Normalize(15.0);
Coordinates i2 = alp.Final.Position;
Coordinates i3 = i2 + alp.Final.NextPartition.Vector().Normalize(15.0);
LinePath il12 = new LinePath(new Coordinates[] { i1, i2 });
LinePath il23 = new LinePath(new Coordinates[] { i2, i3 });
LinePath il13 = new LinePath(new Coordinates[] { i1, i3 });
Coordinates iCC = il13.GetClosestPoint(i2).Location;
if (GeneralToolkit.TriangleArea(i1, i2, i3) < 0)
{
il13 = il13.ShiftLateral(iCC.DistanceTo(i2) + alp.Lane.Width / 2.0);
}
else
{
il13 = il13.ShiftLateral(-iCC.DistanceTo(i2) + alp.Lane.Width / 2.0);
}
LinePath.PointOnPath iCCP = il13.GetClosestPoint(iCC);
iCCP = il13.AdvancePoint(iCCP, -10.0);
il13 = il13.SubPath(iCCP, 20.0);
Polygon iBase = GenerateSimplePolygon(il12, alp.Lane.Width);
iBase.Add(il13[0]);
Polygon iP = Polygon.GrahamScan(iBase);
pBase = PolygonToolkit.PolygonUnion(new List <Polygon>(new Polygon[] { pBase, iP }));
}
return(pBase);
}
// fall out
return(null);
}
/// <summary>
/// Behavior we would like to do other than going straight
/// </summary>
/// <param name="arbiterLane"></param>
/// <param name="vehicleState"></param>
/// <param name="p"></param>
/// <param name="blockages"></param>
/// <returns></returns>
/// <remarks>tries to go right, if not goest left if needs
/// to if forward vehicle ahead and we're stopped because of them</remarks>
public Maneuver?SecondaryManeuver(ArbiterLane arbiterLane, ArbiterLane closestGood, VehicleState vehicleState, List <ITacticalBlockage> blockages,
LaneChangeParameters?entryParameters)
{
// check blockages
if (blockages != null && blockages.Count > 0 && blockages[0] is OpposingLaneBlockage)
{
// create the blockage state
EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);
// go to a blockage handling tactical
return(new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
// check dist needed to complete
double neededDistance = (Math.Abs(arbiterLane.LaneId.Number - closestGood.LaneId.Number) * 1.5 * TahoeParams.VL) +
(-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration));
// get upper bound
LinePath.PointOnPath xFront = arbiterLane.LanePath().GetClosestPoint(vehicleState.Front);
Coordinates xUpper = arbiterLane.LanePath().AdvancePoint(xFront, -neededDistance).Location;
if (entryParameters.HasValue)
{
// check if we should get back, keep speed nice n' lowi fpassing failed
if (entryParameters.Value.Reason == LaneChangeReason.FailedForwardVehicle)
{
double xToReturn = arbiterLane.DistanceBetween(entryParameters.Value.DefaultReturnLowerBound, vehicleState.Front);
if (xToReturn >= 0.0)
{
ArbiterOutput.Output("Distance until must return to lane: " + xToReturn);
}
else
{
ArbiterOutput.Output("Can return to lane from arbitrary upper bound: " + xToReturn);
}
// check can return
if (xToReturn < 0)
{
// check if right lateral exists exactly here
if (this.rightLateralReasoning.ExistsExactlyHere(vehicleState) && this.rightLateralReasoning.LateralLane.Equals(closestGood))
{
ArbiterOutput.Output("Right lateral reasoning good and exists exactly here");
return(this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, true));
}
else if (!this.rightLateralReasoning.ExistsRelativelyHere(vehicleState) && !this.rightLateralReasoning.LateralLane.Equals(closestGood))
{
ArbiterOutput.Output("Right lateral reasoning not good closest and does not exist here");
if (this.secondaryLateralReasoning == null || !this.secondaryLateralReasoning.LateralLane.Equals(closestGood))
{
this.secondaryLateralReasoning = new LateralReasoning(closestGood, UrbanChallenge.Common.Sensors.SideObstacleSide.Passenger);
}
if (this.secondaryLateralReasoning.ExistsExactlyHere(vehicleState))
{
ILateralReasoning tmpReasoning = this.rightLateralReasoning;
this.rightLateralReasoning = this.secondaryLateralReasoning;
Maneuver?tmp = this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, true);
this.rightLateralReasoning = tmpReasoning;
return(tmp);
}
else
{
ArbiterOutput.Output("Cosest good lane does not exist here??");
return(null);
}
}
else
{
ArbiterOutput.Output("Can't change lanes!!, RL exists exactly: " + this.rightLateralReasoning.ExistsExactlyHere(vehicleState).ToString() +
", RL exists rel: " + this.rightLateralReasoning.ExistsRelativelyHere(vehicleState).ToString() + ", RL closest good: " + this.rightLateralReasoning.LateralLane.Equals(closestGood).ToString());
return(null);
}
}
else
{
return(null);
}
}
}
// lane change info
LaneChangeInformation lci = new LaneChangeInformation(LaneChangeReason.Navigation, null);
// notify
ArbiterOutput.Output("In Opposing with no Previous state knowledge, attempting to return");
// check if right lateral exists exactly here
if (this.rightLateralReasoning.ExistsExactlyHere(vehicleState) && this.rightLateralReasoning.LateralLane.Equals(closestGood))
{
ArbiterOutput.Output("Right lateral reasoning good and exists exactly here");
return(this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, false));
}
else if (!this.rightLateralReasoning.ExistsRelativelyHere(vehicleState) && !this.rightLateralReasoning.LateralLane.Equals(closestGood))
{
ArbiterOutput.Output("Right lateral reasoning not good closest and does not exist here");
if (this.secondaryLateralReasoning == null || !this.secondaryLateralReasoning.LateralLane.Equals(closestGood))
{
this.secondaryLateralReasoning = new LateralReasoning(closestGood, UrbanChallenge.Common.Sensors.SideObstacleSide.Passenger);
}
if (this.secondaryLateralReasoning.ExistsExactlyHere(vehicleState))
{
ILateralReasoning tmpReasoning = this.rightLateralReasoning;
this.rightLateralReasoning = this.secondaryLateralReasoning;
Maneuver?tmp = this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, false);
this.rightLateralReasoning = tmpReasoning;
return(tmp);
}
else
{
ArbiterOutput.Output("Cosest good lane does not exist here??");
return(null);
}
}
else
{
ArbiterOutput.Output("Can't change lanes!!, RL exists exactly: " + this.rightLateralReasoning.ExistsExactlyHere(vehicleState).ToString() +
", RL exists rel: " + this.rightLateralReasoning.ExistsRelativelyHere(vehicleState).ToString() + ", RL closest good: " + this.rightLateralReasoning.LateralLane.Equals(closestGood).ToString());
return(null);
}
}
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 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);
}
}
protected void InitializePlanningSettings()
{
settings = new PlanningSettings();
smootherBasePath = null;
maxSmootherBasePathAdvancePoint = LinePath.PointOnPath.Invalid;
avoidanceBasePath = null;
maxAvoidanceBasePathAdvancePoint = LinePath.PointOnPath.Invalid;
smootherTargetPaths = new List<Boundary>();
leftLaneBounds = new List<Boundary>();
rightLaneBounds = new List<Boundary>();
additionalLeftBounds = new List<Boundary>();
additionalRightBounds = new List<Boundary>();
extraObstacles = new List<Obstacle>();
disablePathAngleCheck = false;
pathAngleCheckMax = 20;
pathAngleMax = 15 * Math.PI / 180.0;
useAvoidancePath = false;
laneWidthAtPathEnd = 3.5;
settings.Options.alpha_w = 0.00025;
settings.Options.alpha_d = 10;
settings.Options.alpha_c = 10;
}
private void ShiftNetworkOkButton_Click(object sender, EventArgs e)
{
try
{
double east = double.Parse(this.ShiftNetworkEastTextBox.Text);
double north = double.Parse(this.ShiftNetworkNorthTextBox.Text);
Coordinates shift = new Coordinates(east, north);
foreach (IArbiterWaypoint iaw in roads.ArbiterWaypoints.Values)
{
iaw.Position = iaw.Position + shift;
}
// safety zone filter
DisplayObjectFilter f = delegate(IDisplayObject target)
{
// check if target is network object
if (target is ArbiterSafetyZone)
{
return(true);
}
else
{
return(false);
}
};
// remove safety zones
display.RemoveDisplayObjectType(f);
// new safety
roads.ArbiterSafetyZones = new List <ArbiterSafetyZone>();
// remove from network
List <IDisplayObject> displayObjects = new List <IDisplayObject>();
foreach (IDisplayObject ido in roads.DisplayObjects)
{
if (!f(ido))
{
displayObjects.Add(ido);
}
}
// remove lane safety zones, create new partition paths
foreach (ArbiterSegment asg in roads.ArbiterSegments.Values)
{
foreach (ArbiterLane al in asg.Lanes.Values)
{
al.SafetyZones = new List <ArbiterSafetyZone>();
// path segments of lane
List <IPathSegment> pathSegments = new List <IPathSegment>();
// loop
foreach (ArbiterLanePartition alPar in al.Partitions)
{
// make new segment
pathSegments.Add(new LinePathSegment(alPar.Initial.Position, alPar.Final.Position));
}
// generate lane partition path
Path partitionPath = new Path(pathSegments);
al.PartitionPath = partitionPath;
}
}
// recreate safety zones
foreach (IArbiterWaypoint iaw in roads.ArbiterWaypoints.Values)
{
if (iaw is ArbiterWaypoint)
{
ArbiterWaypoint aw = (ArbiterWaypoint)iaw;
if (aw.IsStop)
{
ArbiterLane al = aw.Lane;
LinePath.PointOnPath end = al.GetClosestPoint(aw.Position);
double dist = -30;
LinePath.PointOnPath begin = al.LanePath().AdvancePoint(end, ref dist);
if (dist != 0)
{
EditorOutput.WriteLine(aw.ToString() + " safety zone too close to start of lane, setting start to start of lane");
begin = al.LanePath().StartPoint;
}
ArbiterSafetyZone asz = new ArbiterSafetyZone(al, end, begin);
asz.isExit = true;
asz.Exit = aw;
al.SafetyZones.Add(asz);
roads.DisplayObjects.Add(asz);
roads.ArbiterSafetyZones.Add(asz);
display.AddDisplayObject(asz);
}
}
}
// redraw
this.display.Invalidate();
// notify
EditorOutput.WriteLine("Shifted road network: east: " + east.ToString("F6") + ", north: " + north.ToString("F6"));
EditorOutput.WriteLine("Recomputed position-dependent types");
// close form
this.Close();
}
catch (Exception ex)
{
EditorOutput.WriteLine("Shift of road network failed: \n" + ex.ToString());
}
}
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;
}