public void Initialize(Behavior b)
{
SimpleStayInLaneBehavior sb = (SimpleStayInLaneBehavior)b;
// store the base path
basePath = ConvertPath(sb.BasePath);
// get the lower, upper bound
leftBound = FindBoundary(sb.LaneWidth / 2);
rightBound = FindBoundary(-sb.LaneWidth / 2);
// convert everything to be vehicle relative
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer();
pathTime = absTransform.Timestamp;
basePath = basePath.Transform(absTransform);
leftBound = leftBound.Transform(absTransform);
rightBound = rightBound.Transform(absTransform);
// send the left and right bounds
Services.UIService.PushRelativePath(leftBound, pathTime, "left bound");
Services.UIService.PushRelativePath(rightBound, pathTime, "right bound");
maxSpeed = sb.MaxSpeed;
obstacleManager = new ObstacleManager(); // hik
}
public void Initialize(Behavior b)
{
SimpleStayInLaneBehavior sb = (SimpleStayInLaneBehavior)b;
// store the base path
basePath = ConvertPath(sb.BasePath);
// get the lower, upper bound
leftBound = FindBoundary(sb.LaneWidth/2);
rightBound = FindBoundary(-sb.LaneWidth/2);
// convert everything to be vehicle relative
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer();
pathTime = absTransform.Timestamp;
basePath = basePath.Transform(absTransform);
leftBound = leftBound.Transform(absTransform);
rightBound = rightBound.Transform(absTransform);
// send the left and right bounds
Services.UIService.PushRelativePath(leftBound, pathTime, "left bound");
Services.UIService.PushRelativePath(rightBound, pathTime, "right bound");
maxSpeed = sb.MaxSpeed;
obstacleManager = new ObstacleManager(); // hik
}
public LinePath LinearizeCenterLine(LinearizationOptions options)
{
LinePath transformedPath = centerlinePath;
if (options.Timestamp.IsValid)
{
RelativeTransform relTransform = Services.RelativePose.GetTransform(timestamp, options.Timestamp);
OperationalTrace.WriteVerbose("in LinearizeCenterLine, tried to find {0}->{1}, got {2}->{3}", timestamp, options.Timestamp, relTransform.OriginTimestamp, relTransform.EndTimestamp);
transformedPath = centerlinePath.Transform(relTransform);
}
LinePath.PointOnPath startPoint = transformedPath.AdvancePoint(centerlinePath.ZeroPoint, options.StartDistance);
LinePath subPath = new LinePath();;
if (options.EndDistance > options.StartDistance)
{
subPath = transformedPath.SubPath(startPoint, options.EndDistance - options.StartDistance);
}
if (subPath.Count < 2)
{
subPath.Clear();
Coordinates startPt = startPoint.Location;
subPath.Add(startPt);
subPath.Add(centerlinePath.GetSegment(startPoint.Index).UnitVector *Math.Max(options.EndDistance - options.StartDistance, 0.1) + startPt);
}
return(subPath);
}
public override void Initialize(Behavior b)
{
base.Initialize(b);
Services.ObstaclePipeline.ExtraSpacing = 0;
Services.ObstaclePipeline.UseOccupancyGrid = true;
// extract the relevant information
TurnBehavior cb = (TurnBehavior)b;
targetLaneID = cb.TargetLane;
// create a fake start lane so we can do the intersection pull stuff
pseudoStartLane = new LinePath();
pseudoStartLane.Add(new Coordinates(-1, 0));
pseudoStartLane.Add(Coordinates.Zero);
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(cb.TimeStamp);
pseudoStartLane = pseudoStartLane.Transform(absTransform.Invert());
HandleTurnBehavior(cb);
curTimestamp = cb.TimeStamp;
// do an initial plan without obstacle avoidance
DoInitialPlan();
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "turn behavior - initialized");
}
public LinePath LinearizeRightBound(LinearizationOptions options)
{
LinePath bound = rightBound;
if (options.Timestamp.IsValid)
{
RelativeTransform relTransform = Services.RelativePose.GetTransform(timestamp, options.Timestamp);
bound = bound.Transform(relTransform);
}
if (options.EndDistance > options.StartDistance)
{
// TODO: work off centerline path
LinePath.PointOnPath startPoint = bound.AdvancePoint(bound.ZeroPoint, options.StartDistance);
return(bound.SubPath(startPoint, options.EndDistance - options.StartDistance).ShiftLateral(options.LaneShiftDistance));
}
else
{
return(new LinePath());
}
}
public 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 override void Process(object param)
{
try {
Trace.CorrelationManager.StartLogicalOperation("ChangeLanes");
if (!base.BeginProcess())
{
return;
}
// check if we were given a parameter
if (param != null && param is ChangeLaneBehavior)
{
ChangeLaneBehavior clParam = (ChangeLaneBehavior)param;
HandleBehavior(clParam);
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "got new param -- speed {0}, dist {1}, dist timestamp {2}", clParam.SpeedCommand, clParam.MaxDist, clParam.TimeStamp);
}
// project the lane paths up to the current time
RelativeTransform relTransform = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
LinePath curStartingPath = startingLanePath.Transform(relTransform);
LinePath curEndingPath = endingLanePath.Transform(relTransform);
// get the starting and ending lane models
ILaneModel startingLaneModel, endingLaneModel;
Services.RoadModelProvider.GetLaneChangeModels(curStartingPath, startingLaneWidth, startingNumLanesLeft, startingNumLanesRight,
curEndingPath, endingLaneWidth, changeLeft, behaviorTimestamp, out startingLaneModel, out endingLaneModel);
// calculate the max speed
// TODO: make this look ahead for slowing down
settings.maxSpeed = GetMaxSpeed(endingLanePath, endingLanePath.ZeroPoint);
// get the remaining lane change distance
double remainingDist = GetRemainingLaneChangeDistance();
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "remaining distance is {0}", remainingDist);
if (cancelled)
{
return;
}
// check if we're done
if (remainingDist <= 0)
{
// create a new stay in lane behavior
int deltaLanes = changeLeft ? -1 : 1;
StayInLaneBehavior stayInLane = new StayInLaneBehavior(endingLaneID, speedCommand, null, endingLanePath, endingLaneWidth, startingNumLanesLeft + deltaLanes, startingNumLanesRight - deltaLanes);
stayInLane.TimeStamp = behaviorTimestamp.ts;
Services.BehaviorManager.Execute(stayInLane, null, false);
// send completion report
ForwardCompletionReport(new SuccessCompletionReport(typeof(ChangeLaneBehavior)));
return;
}
// calculate the planning distance
double planningDist = GetPlanningDistance();
if (planningDist < remainingDist + TahoeParams.VL)
{
planningDist = remainingDist + TahoeParams.VL;
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "planning dist {0}", planningDist);
// create a linearization options structure
LinearizationOptions laneOpts = new LinearizationOptions();
laneOpts.Timestamp = curTimestamp;
// get the center line of the target lane starting at the distance we want to enter into it
laneOpts.StartDistance = remainingDist;
laneOpts.EndDistance = planningDist;
LinePath centerLine = endingLaneModel.LinearizeCenterLine(laneOpts);
// add the final center line as a weighting
AddTargetPath(centerLine.Clone(), default_lane_alpha_w);
// pre-pend (0,0) as our position
centerLine.Insert(0, new Coordinates(0, 0));
LinePath leftBound = null;
LinePath rightBound = null;
// figure out if the lane is to the right or left of us
laneOpts.EndDistance = planningDist;
double leftEndingStartDist, rightEndingStartDist;
GetLaneBoundStartDists(curEndingPath, endingLaneWidth, out leftEndingStartDist, out rightEndingStartDist);
if (changeLeft)
{
// we're the target lane is to the left
// get the left bound of the target lane
laneOpts.StartDistance = Math.Max(leftEndingStartDist, TahoeParams.FL);
leftBound = endingLaneModel.LinearizeLeftBound(laneOpts);
}
else
{
// we're changing to the right, get the right bound of the target lane
laneOpts.StartDistance = Math.Max(rightEndingStartDist, TahoeParams.FL);
rightBound = endingLaneModel.LinearizeRightBound(laneOpts);
}
// get the other bound as the starting lane up to 5m before the remaining dist
laneOpts.StartDistance = TahoeParams.FL;
laneOpts.EndDistance = Math.Max(0, remainingDist - 5);
if (changeLeft)
{
if (laneOpts.EndDistance > 0)
{
rightBound = startingLaneModel.LinearizeRightBound(laneOpts);
}
else
{
rightBound = new LinePath();
}
}
else
{
if (laneOpts.EndDistance > 0)
{
leftBound = startingLaneModel.LinearizeLeftBound(laneOpts);
}
else
{
leftBound = new LinePath();
}
}
// append on the that bound of the target lane starting at the remaining dist
laneOpts.StartDistance = Math.Max(remainingDist, TahoeParams.FL);
laneOpts.EndDistance = planningDist;
if (changeLeft)
{
rightBound.AddRange(endingLaneModel.LinearizeRightBound(laneOpts));
}
else
{
leftBound.AddRange(endingLaneModel.LinearizeLeftBound(laneOpts));
}
// set up the planning
smootherBasePath = centerLine;
AddLeftBound(leftBound, !changeLeft);
AddRightBound(rightBound, changeLeft);
Services.UIService.PushLineList(centerLine, curTimestamp, "subpath", true);
Services.UIService.PushLineList(leftBound, curTimestamp, "left bound", true);
Services.UIService.PushLineList(rightBound, curTimestamp, "right bound", true);
if (cancelled)
{
return;
}
// set auxillary options
settings.endingHeading = centerLine.EndSegment.UnitVector.ArcTan;
// smooth and track that stuff
SmoothAndTrack(commandLabel, true);
}
finally {
Trace.CorrelationManager.StopLogicalOperation();
}
}
public override void Process(object param)
{
if (!base.BeginProcess())
{
return;
}
// check if we're given a param
if (param != null && param is TurnBehavior)
{
TurnBehavior turnParam = (TurnBehavior)param;
HandleTurnBehavior(turnParam);
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "turn behavior - got new param, speed command {0}", turnParam.SpeedCommand);
}
LinePath curTargetPath = targetPath.Clone();
LinePath curLeftBound = null;
if (leftBound != null)
{
curLeftBound = leftBound.Clone();
}
LinePath curRightBound = null;
if (rightBound != null)
{
curRightBound = rightBound.Clone();
}
// transform the path into the current timestamp
if (behaviorTimestamp != curTimestamp)
{
// get the relative transform
RelativeTransform relTransform = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "in turn behavior, transforming from {0}->{1}, wanted {2}->{3}", relTransform.OriginTimestamp, relTransform.EndTimestamp, behaviorTimestamp, curTimestamp);
curTargetPath.TransformInPlace(relTransform);
if (curLeftBound != null)
{
curLeftBound.TransformInPlace(relTransform);
}
if (curRightBound != null)
{
curRightBound.TransformInPlace(relTransform);
}
}
// get the distance between our current location and the start point
double distToStart = Coordinates.Zero.DistanceTo(curTargetPath[0]);
double startDist = Math.Max(0, TahoeParams.FL - distToStart);
// extract off the first 5 m of the target path
LinePath.PointOnPath endTarget = curTargetPath.AdvancePoint(curTargetPath.StartPoint, TahoeParams.VL);
curTargetPath = curTargetPath.SubPath(curTargetPath.StartPoint, endTarget);
curTargetPath = curTargetPath.RemoveZeroLengthSegments();
// adjust the left bound and right bounds starting distance
if (curLeftBound != null)
{
LinePath.PointOnPath leftBoundStart = curLeftBound.AdvancePoint(curLeftBound.StartPoint, 2);
curLeftBound = curLeftBound.SubPath(leftBoundStart, curLeftBound.EndPoint);
AddLeftBound(curLeftBound, false);
Services.UIService.PushLineList(curLeftBound, curTimestamp, "left bound", true);
}
if (curRightBound != null)
{
LinePath.PointOnPath rightBoundStart = curRightBound.AdvancePoint(curRightBound.StartPoint, 2);
curRightBound = curRightBound.SubPath(rightBoundStart, curRightBound.EndPoint);
AddRightBound(curRightBound, false);
Services.UIService.PushLineList(curRightBound, curTimestamp, "right bound", true);
}
if (cancelled)
{
return;
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "in turn behavior, dist to start: {0}", distToStart);
if (distToStart < TahoeParams.FL * 0.75)
{
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "in turn behavior, past start point of target path");
// return a completion report
ForwardCompletionReport(new SuccessCompletionReport(typeof(TurnBehavior)));
}
AddTargetPath(curTargetPath.Clone(), 0.005);
// transform the pseudo start lane to the current timestamp
AbsoluteTransformer trans = Services.StateProvider.GetAbsoluteTransformer(curTimestamp);
LinePath curStartPath = pseudoStartLane.Transform(trans);
// add the intersection pull path
LinePath intersectionPullPath = new LinePath();
double pullWeight = 0;
GetIntersectionPullPath(curStartPath, curTargetPath, intersectionPolygon, true, true, intersectionPullPath, ref pullWeight);
if (intersectionPullPath.Count > 0)
{
AddTargetPath(intersectionPullPath, pullWeight);
}
// set up planning details
// add our position to the current target path
LinePath origTargetPath = curTargetPath.Clone();
curTargetPath.Insert(0, new Coordinates(0, 0));
//curTargetPath.Insert(1, new Coordinates(1, 0));
smootherBasePath = curTargetPath;
// add the bounds
// calculate max speed
settings.maxSpeed = GetMaxSpeed(null, LinePath.PointOnPath.Invalid);
settings.Options.w_diff = 4;
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "max speed set to {0}", settings.maxSpeed);
if (cancelled)
{
return;
}
Services.UIService.PushLineList(curTargetPath, curTimestamp, "subpath", true);
// set the avoidance path to the previously planned path
// transform to the current timestamp
disablePathAngleCheck = true;
if (turnBasePath != null)
{
avoidanceBasePath = turnBasePath.Transform(trans);
avoidanceBasePath = avoidanceBasePath.SubPath(avoidanceBasePath.GetClosestPoint(Coordinates.Zero), avoidanceBasePath.EndPoint);
maxAvoidanceBasePathAdvancePoint = avoidanceBasePath.GetPointOnPath(1);
if (avoidanceBasePath.PathLength > 7.5)
{
smootherBasePath = avoidanceBasePath.SubPath(avoidanceBasePath.StartPoint, avoidanceBasePath.AdvancePoint(avoidanceBasePath.EndPoint, -7.5));
}
else
{
smootherBasePath = avoidanceBasePath.Clone();
}
disablePathAngleCheck = false;
}
// fill in auxiliary settings
if (curLeftBound != null || curRightBound != null)
{
settings.endingHeading = curTargetPath.EndSegment.UnitVector.ArcTan;
}
settings.endingPositionFixed = false;
settings.endingPositionMax = 16;
settings.endingPositionMin = -16;
if (curLeftBound != null && curLeftBound.Count > 0 && curTargetPath.Count > 0)
{
double leftWidth = curLeftBound[0].DistanceTo(origTargetPath[0]) - TahoeParams.T / 2;
settings.endingPositionMax = leftWidth;
settings.endingPositionFixed = true;
}
if (curRightBound != null && curRightBound.Count > 0 && curTargetPath.Count > 0)
{
double rightWidth = curRightBound[0].DistanceTo(origTargetPath[0]) - TahoeParams.T / 2;
settings.endingPositionFixed = true;
settings.endingPositionMin = -rightWidth;
}
//disablePathAngleCheck = true;
//useAvoidancePath = true;
// do the planning
SmoothAndTrack(commandLabel, true);
}
private void DoInitialPlan()
{
InitializePlanningSettings();
curTimestamp = Services.RelativePose.CurrentTimestamp;
vs = Services.StateProvider.GetVehicleState();
LinePath curTargetPath = targetPath.Clone();
LinePath curLeftBound = null;
if (leftBound != null)
{
curLeftBound = leftBound.Clone();
}
LinePath curRightBound = null;
if (rightBound != null)
{
curRightBound = rightBound.Clone();
}
// get the distance between our current location and the start point
double distToStart = Coordinates.Zero.DistanceTo(curTargetPath[0]);
// extract off the first 5 m of the target path
LinePath.PointOnPath endTarget = curTargetPath.AdvancePoint(curTargetPath.StartPoint, 12);
curTargetPath = curTargetPath.SubPath(curTargetPath.StartPoint, endTarget);
AddTargetPath(curTargetPath.Clone(), 0.005);
// adjust the left bound and right bounds starting distance
if (curLeftBound != null)
{
LinePath.PointOnPath leftBoundStart = curLeftBound.AdvancePoint(curLeftBound.StartPoint, 2);
curLeftBound = curLeftBound.SubPath(leftBoundStart, curLeftBound.EndPoint);
AddLeftBound(curLeftBound, false);
}
if (curRightBound != null)
{
LinePath.PointOnPath rightBoundStart = curRightBound.AdvancePoint(curRightBound.StartPoint, 2);
curRightBound = curRightBound.SubPath(rightBoundStart, curRightBound.EndPoint);
AddRightBound(curRightBound, false);
}
// add the intersection pull path
LinePath intersectionPullPath = new LinePath();
double pullWeight = 0;
AbsoluteTransformer trans = Services.StateProvider.GetAbsoluteTransformer(curTimestamp);
GetIntersectionPullPath(pseudoStartLane.Transform(trans), curTargetPath, intersectionPolygon, true, true, intersectionPullPath, ref pullWeight);
if (intersectionPullPath.Count > 0)
{
AddTargetPath(intersectionPullPath, pullWeight);
}
// set up planning details
// add our position to the current target path
curTargetPath.Insert(0, new Coordinates(0, 0));
smootherBasePath = curTargetPath;
// add the bounds
// calculate max speed
settings.maxSpeed = GetMaxSpeed(null, LinePath.PointOnPath.Invalid);
// fill in auxiliary settings
if (curLeftBound != null || curRightBound != null)
{
settings.endingHeading = curTargetPath.EndSegment.UnitVector.ArcTan;
}
disablePathAngleCheck = true;
settings.Options.w_diff = 4;
// do the planning
PlanningResult planningResult = Smooth(false);
// if the planning was a success, store the result
if (!planningResult.dynamicallyInfeasible)
{
// transform to absolute coordinates
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp);
turnBasePath = planningResult.smoothedPath.Transform(absTransform.Invert());
}
}
public override void Process(object param)
{
// inform the base that we're beginning processing
if (!BeginProcess())
{
return;
}
// check if we were given a parameter
if (param != null && param is StayInLaneBehavior)
{
HandleBehavior((StayInLaneBehavior)param);
}
if (reverseGear)
{
ProcessReverse();
return;
}
// figure out the planning distance
double planningDistance = GetPlanningDistance();
if (sparse && planningDistance > 10)
{
planningDistance = 10;
}
double?boundStartDistMin = null;
double?boundEndDistMax = null;
if (laneID != null && Services.RoadNetwork != null)
{
ArbiterLane lane = Services.RoadNetwork.ArbiterSegments[laneID.SegmentId].Lanes[laneID];
AbsolutePose pose = Services.StateProvider.GetAbsolutePose();
ArbiterLanePartition partition = lane.GetClosestPartition(pose.xy);
LinePath partitionPath = partition.UserPartitionPath;
LinePath.PointOnPath closestPoint = partitionPath.GetClosestPoint(pose.xy);
double remainingDist = planningDistance;
double totalDist = partitionPath.DistanceBetween(closestPoint, partitionPath.EndPoint);
remainingDist -= totalDist;
if (sparse)
{
// walk ahead and determine where sparsity ends
bool nonSparseFound = false;
while (remainingDist > 0)
{
// get the next partition
partition = partition.Final.NextPartition;
if (partition == null)
{
break;
}
if (partition.Type != PartitionType.Sparse)
{
nonSparseFound = true;
break;
}
else
{
double dist = partition.UserPartitionPath.PathLength;
totalDist += dist;
remainingDist -= dist;
}
}
if (nonSparseFound)
{
boundStartDistMin = totalDist;
}
}
else
{
// determine if there is a sparse segment upcoming
bool sparseFound = false;
while (remainingDist > 0)
{
partition = partition.Final.NextPartition;
if (partition == null)
{
break;
}
if (partition.Type == PartitionType.Sparse)
{
sparseFound = true;
break;
}
else
{
double dist = partition.Length;
totalDist += dist;
remainingDist -= dist;
}
}
if (sparseFound)
{
boundEndDistMax = totalDist;
sparse = true;
}
}
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "in stay in lane, planning distance {0}", planningDistance);
// update the rndf path
RelativeTransform relTransfrom = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
LinePath curRndfPath = rndfPath.Transform(relTransfrom);
ILaneModel centerLaneModel = Services.RoadModelProvider.GetLaneModel(curRndfPath, rndfPathWidth + extraWidth, curTimestamp, numLanesLeft, numLanesRight);
double avoidanceExtra = sparse ? 5 : 7.5;
LinePath centerLine, leftBound, rightBound;
if (boundEndDistMax != null || boundStartDistMin != null)
{
LinearizeStayInLane(centerLaneModel, planningDistance + avoidanceExtra, null, boundEndDistMax, boundStartDistMin, null, curTimestamp, out centerLine, out leftBound, out rightBound);
}
else
{
LinearizeStayInLane(centerLaneModel, planningDistance + avoidanceExtra, curTimestamp, out centerLine, out leftBound, out rightBound);
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "in stay in lane, center line count: {0}, left bound count: {1}, right bound count: {2}", centerLine.Count, leftBound.Count, rightBound.Count);
// calculate time to collision of opposing obstacle if it exists
LinePath targetLine = centerLine;
double targetWeight = default_lane_alpha_w;
if (sparse)
{
targetWeight = 0.00000025;
}
else if (oncomingVehicleExists)
{
double shiftDist = -(TahoeParams.T + 1);
targetLine = centerLine.ShiftLateral(shiftDist);
targetWeight = 0.01;
}
else if (opposingLaneVehicleExists)
{
double timeToCollision = opposingLaneVehicleDist / (Math.Abs(vs.speed) + Math.Abs(opposingLaneVehicleSpeed));
Services.Dataset.ItemAs <double>("time to collision").Add(timeToCollision, curTimestamp);
if (timeToCollision < 5)
{
// shift to the right
double shiftDist = -TahoeParams.T / 2.0;
targetLine = centerLine.ShiftLateral(shiftDist);
}
}
// set up the planning
AddTargetPath(targetLine, targetWeight);
if (!sparse || boundStartDistMin != null || boundEndDistMax != null)
{
AddLeftBound(leftBound, true);
if (!oncomingVehicleExists)
{
AddRightBound(rightBound, false);
}
}
double targetDist = Math.Max(centerLine.PathLength - avoidanceExtra, planningDistance);
smootherBasePath = centerLine.SubPath(centerLine.StartPoint, targetDist);
maxSmootherBasePathAdvancePoint = smootherBasePath.EndPoint;
double extraDist = (planningDistance + avoidanceExtra) - centerLine.PathLength;
extraDist = Math.Min(extraDist, 5);
if (extraDist > 0)
{
centerLine.Add(centerLine[centerLine.Count - 1] + centerLine.EndSegment.Vector.Normalize(extraDist));
}
avoidanceBasePath = centerLine;
Services.UIService.PushLineList(centerLine, curTimestamp, "subpath", true);
Services.UIService.PushLineList(leftBound, curTimestamp, "left bound", true);
Services.UIService.PushLineList(rightBound, curTimestamp, "right bound", true);
// get the overall max speed looking forward from our current point
settings.maxSpeed = GetMaxSpeed(curRndfPath, curRndfPath.AdvancePoint(curRndfPath.ZeroPoint, vs.speed * TahoeParams.actuation_delay));
// get the max speed at the end point
settings.maxEndingSpeed = GetMaxSpeed(curRndfPath, curRndfPath.AdvancePoint(curRndfPath.ZeroPoint, planningDistance));
useAvoidancePath = false;
if (sparse)
{
// limit to 5 mph
laneWidthAtPathEnd = 20;
pathAngleCheckMax = 50;
pathAngleMax = 5 * Math.PI / 180.0;
settings.maxSpeed = Math.Min(settings.maxSpeed, 2.2352);
maxAvoidanceBasePathAdvancePoint = avoidanceBasePath.AdvancePoint(avoidanceBasePath.EndPoint, -2);
//maxSmootherBasePathAdvancePoint = smootherBasePath.AdvancePoint(smootherBasePath.EndPoint, -2);
LinePath leftEdge = RoadEdge.GetLeftEdgeLine();
LinePath rightEdge = RoadEdge.GetRightEdgeLine();
if (leftEdge != null)
{
leftLaneBounds.Add(new Boundary(leftEdge, 0.1, 1, 100, false));
}
if (rightEdge != null)
{
rightLaneBounds.Add(new Boundary(rightEdge, 0.1, 1, 100, false));
}
PlanningResult result = new PlanningResult();
ISteeringCommandGenerator commandGenerator = SparseArcVoting.SparcVote(ref prevCurvature, goalPoint);
if (commandGenerator == null)
{
// we have a block, report dynamically infeasible
result = OnDynamicallyInfeasible(null, null);
}
else
{
result.steeringCommandGenerator = commandGenerator;
result.commandLabel = commandLabel;
}
Track(result, commandLabel);
return;
}
else if (oncomingVehicleExists)
{
laneWidthAtPathEnd = 10;
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "max speed set to {0}", settings.maxSpeed);
disablePathAngleCheck = false;
SmoothAndTrack(commandLabel, true);
}
public override void Process(object param)
{
if (!base.BeginProcess())
{
return;
}
if (param is ZoneTravelingBehavior)
{
HandleBaseBehavior((ZoneTravelingBehavior)param);
}
extraObstacles = GetPerimeterObstacles();
if (reverseGear)
{
ProcessReverse();
return;
}
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp);
// get the vehicle relative path
LinePath relRecommendedPath = recommendedPath.Transform(absTransform);
LinePath.PointOnPath zeroPoint = relRecommendedPath.ZeroPoint;
// get the distance to the end point
double distToEnd = relRecommendedPath.DistanceBetween(zeroPoint, relRecommendedPath.EndPoint);
// get the planning distance
double planningDist = GetPlanningDistance();
planningDist = Math.Max(planningDist, 20);
planningDist -= zeroPoint.Location.Length;
if (planningDist < 2.5)
{
planningDist = 2.5;
}
if (distToEnd < planningDist)
{
// make the speed command at stop speed command
behaviorTimestamp = curTimestamp;
speedCommand = new StopAtDistSpeedCommand(distToEnd - TahoeParams.FL);
planningDist = distToEnd;
approachSpeed = recommendedSpeed.Speed;
settings.endingHeading = relRecommendedPath.EndSegment.UnitVector.ArcTan;
settings.endingPositionFixed = true;
settings.endingPositionMax = 2;
settings.endingPositionMin = -2;
}
else
{
speedCommand = new ScalarSpeedCommand(recommendedSpeed.Speed);
}
// get the distance of the path segment we care about
LinePath pathSegment = relRecommendedPath.SubPath(zeroPoint, planningDist);
double avoidanceDist = planningDist + 5;
avoidanceBasePath = relRecommendedPath.SubPath(zeroPoint, ref avoidanceDist);
if (avoidanceDist > 0)
{
avoidanceBasePath.Add(avoidanceBasePath.EndPoint.Location + avoidanceBasePath.EndSegment.Vector.Normalize(avoidanceDist));
}
// test if we should clear out of arc mode
if (arcMode)
{
if (TestNormalModeClear(relRecommendedPath, zeroPoint))
{
prevCurvature = double.NaN;
arcMode = false;
}
}
if (Math.Abs(zeroPoint.AlongtrackDistance(Coordinates.Zero)) > 1)
{
pathSegment.Insert(0, Coordinates.Zero);
}
else
{
if (pathSegment[0].DistanceTo(pathSegment[1]) < 1)
{
pathSegment.RemoveAt(0);
}
pathSegment[0] = Coordinates.Zero;
}
if (arcMode)
{
Coordinates relativeGoalPoint = relRecommendedPath.EndPoint.Location;
ArcVoteZone(relativeGoalPoint, extraObstacles);
return;
}
double pathLength = pathSegment.PathLength;
if (pathLength < 6)
{
double additionalDist = 6.25 - pathLength;
pathSegment.Add(pathSegment.EndPoint.Location + pathSegment.EndSegment.Vector.Normalize(additionalDist));
}
// determine if polygons are to the left or right of the path
for (int i = 0; i < zoneBadRegions.Length; i++)
{
Polygon poly = zoneBadRegions[i].Transform(absTransform);
int numLeft = 0;
int numRight = 0;
foreach (LineSegment ls in pathSegment.GetSegmentEnumerator())
{
for (int j = 0; j < poly.Count; j++)
{
if (ls.IsToLeft(poly[j]))
{
numLeft++;
}
else
{
numRight++;
}
}
}
if (numLeft > numRight)
{
// we'll consider this polygon on the left of the path
//additionalLeftBounds.Add(new Boundary(poly, 0.1, 0.1, 0));
}
else
{
//additionalRightBounds.Add(new Boundary(poly, 0.1, 0.1, 0));
}
}
// TODO: add zone perimeter
disablePathAngleCheck = false;
laneWidthAtPathEnd = 7;
settings.Options.w_diff = 3;
smootherBasePath = new LinePath();
smootherBasePath.Add(Coordinates.Zero);
smootherBasePath.Add(pathSegment.EndPoint.Location);
AddTargetPath(pathSegment, 0.005);
settings.maxSpeed = recommendedSpeed.Speed;
useAvoidancePath = false;
SmoothAndTrack(command_label, true);
}
