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");
}
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());
}
}