private void HandleBehavior(ChangeLaneBehavior cb)
{
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(cb.TimeStamp);
startingLanePath = cb.BackupStartLanePath.Transform(absTransform);
endingLanePath = cb.BackupTargetLanePath.Transform(absTransform);
startingNumLanesLeft = cb.StartingNumLanesLeft;
startingNumLanesRight = cb.StartingNumLaneRights;
startingLaneWidth = cb.StartLaneWidth;
endingLaneWidth = cb.TargetLaneWidth;
startingLaneID = cb.StartLane;
endingLaneID = cb.TargetLane;
changeLeft = cb.ChangeLeft;
if (cb.MaxDist > -TahoeParams.FL)
{
dist = cb.MaxDist + TahoeParams.FL;
}
else
{
dist = 0;
}
speedCommand = cb.SpeedCommand;
ignorableObstacles = cb.IgnorableObstacles;
Services.ObstaclePipeline.LastIgnoredObstacles = cb.IgnorableObstacles;
behaviorTimestamp = absTransform.Timestamp;
}
protected List <Obstacle> GetObstacles(CarTimestamp curTimestamp)
{
ObstacleCollection col = Services.ObstaclePipeline.GetProcessedObstacles(curTimestamp, SAUDILevel.None);
AbsoluteTransformer transform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp).Invert();
List <Obstacle> ret = new List <Obstacle>(col.obstacles.Count);
foreach (Obstacle obs in col.obstacles)
{
Obstacle newObs = obs.ShallowClone();
if (newObs.cspacePolygon != null)
{
newObs.cspacePolygon = newObs.cspacePolygon.Transform(transform);
}
if (newObs.extrudedPolygon != null)
{
newObs.extrudedPolygon = newObs.extrudedPolygon.Transform(transform);
}
if (newObs.obstaclePolygon != null)
{
newObs.obstaclePolygon = newObs.obstaclePolygon.Transform(transform);
}
if (newObs.predictedPolygon != null)
{
newObs.predictedPolygon = newObs.predictedPolygon.Transform(transform);
}
ret.Add(newObs);
}
return(ret);
}
public void PushPolygons(Polygon[] polygons, CarTimestamp timestamp, string name, bool relative)
{
try {
if (polygons == null)
{
return;
}
if (relative)
{
Polygon[] transPolygons = new Polygon[polygons.Length];
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(timestamp).Invert();
timestamp = absTransform.Timestamp;
for (int i = 0; i < polygons.Length; i++)
{
transPolygons[i] = polygons[i].Transform(absTransform);
}
polygons = transPolygons;
}
Services.Dataset.ItemAs <Polygon[]>(name).Add(polygons, timestamp);
}
catch (Exception ex) {
OperationalLayer.Tracing.OperationalTrace.WriteWarning("could not send polygon data to ui: {0}", ex.Message);
}
}
public void PushObstacles(OperationalObstacle[] obstacles, CarTimestamp timestamp, string name, bool relative)
{
try {
if (obstacles == null || obstacles.Length == 0)
{
return;
}
if (relative)
{
OperationalObstacle[] transformObstacles = new OperationalObstacle[obstacles.Length];
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(timestamp).Invert();
timestamp = absTransform.Timestamp;
for (int i = 0; i < obstacles.Length; i++)
{
transformObstacles[i] = obstacles[i].ShallowClone();
transformObstacles[i].poly = obstacles[i].poly.Transform(absTransform);
}
obstacles = transformObstacles;
}
Services.Dataset.ItemAs <OperationalObstacle[]>(name).Add(obstacles, timestamp);
}
catch (Exception) {
}
}
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);
}
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 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 List <Polygon> GetObstacles(CarTimestamp curTimestamp)
{
int total = 0;
ObstacleCollection obstacles = Services.ObstaclePipeline.GetProcessedObstacles(curTimestamp, Services.BehaviorManager.SAUDILevel);
total += obstacles.obstacles.Count;
if (stayOutPolygons != null)
{
total += stayOutPolygons.Count;
}
List <Polygon> polys = new List <Polygon>(total);
foreach (Obstacle obs in obstacles.obstacles)
{
polys.Add(obs.AvoidancePolygon);
}
// transform the stay-out polygons
if (stayOutPolygons != null)
{
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp);
foreach (Polygon poly in stayOutPolygons)
{
polys.Add(poly.Transform(absTransform));
}
}
return(polys);
}
private void HandleTurnBehavior(TurnBehavior cb)
{
// get the transformer to take us from absolute to relative coordinates
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(cb.TimeStamp);
targetPath = cb.TargetLanePath.Transform(absTransform).RemoveZeroLengthSegments();
if (cb.LeftBound != null)
{
leftBound = new LinePath(cb.LeftBound).Transform(absTransform);
}
else
{
leftBound = null;
}
if (cb.RightBound != null)
{
rightBound = new LinePath(cb.RightBound).Transform(absTransform);
}
else
{
rightBound = null;
}
if (targetPath.PathLength < 12)
{
double pathLength = targetPath.PathLength;
double ext = 12 - pathLength;
targetPath.Add(targetPath.EndPoint.Location + targetPath.EndSegment.Vector.Normalize(ext));
}
behaviorTimestamp = absTransform.Timestamp;
if (cb.IntersectionPolygon != null)
{
intersectionPolygon = cb.IntersectionPolygon.Transform(absTransform);
// set the polygon to the ui
Services.UIService.PushPolygon(cb.IntersectionPolygon, cb.TimeStamp, "intersection polygon", false);
}
else
{
intersectionPolygon = null;
}
this.ignorableObstacles = cb.IgnorableObstacles;
Services.ObstaclePipeline.LastIgnoredObstacles = cb.IgnorableObstacles;
HandleSpeedCommand(cb.SpeedCommand);
}
public void PushRelativePath(LineList list, CarTimestamp timestamp, string name)
{
try {
// convert to the current time
// get the absolute transform
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(timestamp).Invert();
list = list.Transform(absTransform);
Services.Dataset.ItemAs <LineList>(name).Add(list, absTransform.Timestamp);
}
catch (Exception ex) {
OperationalLayer.Tracing.OperationalTrace.WriteWarning("could not send line data to ui: {0}", ex.Message);
}
}
public void PushLineList(LineList list, CarTimestamp timestamp, string name, bool relative)
{
try {
if (relative)
{
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(timestamp).Invert();
timestamp = absTransform.Timestamp;
list = list.Transform(absTransform);
}
Services.Dataset.ItemAs <LineList>(name).Add(list, timestamp);
}
catch (Exception ex) {
OperationalLayer.Tracing.OperationalTrace.WriteWarning("could not send line data to ui: {0}", ex.Message);
}
}
private void ProcessReverse()
{
double planningDistance = reverseDist - Services.TrackedDistance.GetDistanceTravelled(reverseTimestamp, curTimestamp);
// update the rndf path
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp);
// get the vehicle relative path
LinePath relRecommendedPath = recommendedPath.Transform(absTransform);
LinePath targetPath;
if (relRecommendedPath.ZeroPoint.Location.Length > 10)
{
targetPath = new LinePath();
targetPath.Add(new Coordinates(0, 0));
targetPath.Add(new Coordinates(-(planningDistance + TahoeParams.RL + 2), 0));
}
else
{
// get the path in reverse
double dist = -(planningDistance + TahoeParams.RL + 2);
targetPath = relRecommendedPath.SubPath(relRecommendedPath.ZeroPoint, ref dist);
if (dist < 0)
{
targetPath.Add(relRecommendedPath[0] - relRecommendedPath.GetSegment(0).Vector.Normalize(-dist));
}
}
AddTargetPath(targetPath, 0.005);
avoidanceBasePath = targetPath;
double targetDist = Math.Max(targetPath.PathLength - (TahoeParams.RL + 2), planningDistance);
smootherBasePath = new LinePath();
smootherBasePath.Add(Coordinates.Zero);
smootherBasePath.Add(targetPath.AdvancePoint(targetPath.StartPoint, targetDist).Location);
settings.maxSpeed = recommendedSpeed.Speed;
settings.Options.reverse = true;
settings.Options.w_diff = 3;
laneWidthAtPathEnd = 5;
useAvoidancePath = false;
Services.UIService.PushLineList(smootherBasePath, curTimestamp, "subpath", true);
SmoothAndTrack(reverse_label, true);
}
public void PushCircleSegment(CircleSegment circle, CarTimestamp timestamp, string name, bool relative)
{
try {
if (relative)
{
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(timestamp).Invert();
timestamp = absTransform.Timestamp;
circle = circle.Transform(absTransform);
}
Services.Dataset.ItemAs <CircleSegment>(name).Add(circle, timestamp);
}
catch (Exception ex) {
OperationalLayer.Tracing.OperationalTrace.WriteWarning("could not send circle data to ui: {0}", ex.Message);
}
}
public void PushPoint(Coordinates point, CarTimestamp timestamp, string name, bool relative)
{
try {
if (relative)
{
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(timestamp).Invert();
timestamp = absTransform.Timestamp;
point = absTransform.TransformPoint(point);
}
Services.Dataset.ItemAs <Coordinates>(name).Add(point, timestamp);
}
catch (Exception ex) {
OperationalLayer.Tracing.OperationalTrace.WriteWarning("could not send point data to ui: {0}", ex.Message);
}
}
private void HandleBehavior(StayInLaneBehavior b)
{
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(b.TimeStamp);
this.behaviorTimestamp = absTransform.Timestamp;
this.rndfPath = b.BackupPath.Transform(absTransform);
this.rndfPathWidth = b.LaneWidth;
this.numLanesLeft = b.NumLaneLeft;
this.numLanesRight = b.NumLanesRight;
this.laneID = b.TargetLane;
this.ignorableObstacles = b.IgnorableObstacles;
Services.ObstaclePipeline.LastIgnoredObstacles = b.IgnorableObstacles;
HandleSpeedCommand(b.SpeedCommand);
opposingLaneVehicleExists = false;
oncomingVehicleExists = false;
extraWidth = 0;
if (b.Decorators != null)
{
foreach (BehaviorDecorator d in b.Decorators)
{
if (d is OpposingLaneDecorator)
{
opposingLaneVehicleExists = true;
opposingLaneVehicleDist = ((OpposingLaneDecorator)d).Distance;
opposingLaneVehicleSpeed = ((OpposingLaneDecorator)d).Speed;
}
else if (d is OncomingVehicleDecorator)
{
oncomingVehicleExists = true;
oncomingVehicleDist = ((OncomingVehicleDecorator)d).TargetDistance;
oncomingVehicleSpeed = ((OncomingVehicleDecorator)d).TargetSpeed;
oncomingVehicleSpeedCommand = ((OncomingVehicleDecorator)d).SecondarySpeed;
}
else if (d is WidenBoundariesDecorator)
{
extraWidth = ((WidenBoundariesDecorator)d).ExtraWidth;
}
}
}
if (oncomingVehicleExists)
{
double timeToCollision = oncomingVehicleDist / Math.Abs(oncomingVehicleSpeed);
if (oncomingVehicleDist > 30 || timeToCollision > 20 || numLanesRight > 0)
{
oncomingVehicleExists = false;
}
else
{
HandleSpeedCommand(oncomingVehicleSpeedCommand);
}
}
if (laneID != null && Services.RoadNetwork != null)
{
ArbiterLane lane = Services.RoadNetwork.ArbiterSegments[laneID.SegmentId].Lanes[laneID];
AbsolutePose pose = Services.StateProvider.GetAbsolutePose();
sparse = (lane.GetClosestPartition(pose.xy).Type == PartitionType.Sparse);
if (sparse)
{
LinePath.PointOnPath closest = b.BackupPath.GetClosestPoint(pose.xy);
goalPoint = b.BackupPath[closest.Index + 1];
}
}
Services.UIService.PushAbsolutePath(b.BackupPath, b.TimeStamp, "original path1");
Services.UIService.PushAbsolutePath(new LineList(), b.TimeStamp, "original path2");
if (sparse)
{
if (Services.ObstaclePipeline.ExtraSpacing == 0)
{
Services.ObstaclePipeline.ExtraSpacing = 0.5;
}
Services.ObstaclePipeline.UseOccupancyGrid = false;
}
else
{
Services.ObstaclePipeline.ExtraSpacing = 0;
Services.ObstaclePipeline.UseOccupancyGrid = true;
smootherSpacingAdjust = 0;
prevCurvature = double.NaN;
}
}
private List <Obstacle> GetPerimeterObstacles()
{
// transform the polygon to relative coordinates
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer();
Polygon relPerimeter = zonePerimeter.Transform(absTransform);
LinePath relRecommendedPath = recommendedPath.Transform(absTransform);
if (relPerimeter.IsCounterClockwise)
{
relPerimeter = relPerimeter.Reverse();
}
// create a polygon for ourselves and see if we intersect any perimeters
Polygon vehiclePoly = new Polygon();
vehiclePoly.Add(new Coordinates(-TahoeParams.RL, -(TahoeParams.T / 2.0)));
vehiclePoly.Add(new Coordinates(TahoeParams.FL, -(TahoeParams.T / 2.0)));
vehiclePoly.Add(new Coordinates(TahoeParams.FL, TahoeParams.T / 2.0));
vehiclePoly.Add(new Coordinates(-TahoeParams.RL, TahoeParams.T / 2.0));
// inflate by about 2 m
vehiclePoly = vehiclePoly.Inflate(2);
// test if we intersect any of the perimeter points
List <Obstacle> perimeterObstacles = new List <Obstacle>();
List <OperationalObstacle> operationalObstacles = new List <OperationalObstacle>();
List <LineSegment> segments = new List <LineSegment>();
foreach (LineSegment ls1 in relPerimeter.GetSegmentEnumerator())
{
segments.Clear();
if (ls1.Length > 15)
{
// split into multiple segment
double targetLength = 10;
int numSegments = (int)Math.Round(ls1.Length / targetLength);
double splitLength = ls1.Length / numSegments;
Coordinates pt = ls1.P0;
for (int i = 0; i < numSegments; i++)
{
Coordinates endPoint = pt + ls1.Vector.Normalize(splitLength);
LineSegment seg = new LineSegment(pt, endPoint);
segments.Add(seg);
pt = endPoint;
}
}
else
{
segments.Add(ls1);
}
foreach (LineSegment ls in segments)
{
bool pathTooClose = false;
foreach (Coordinates pt in relRecommendedPath)
{
Coordinates closest = ls.ClosestPoint(pt);
if (closest.DistanceTo(pt) < 1)
{
pathTooClose = true;
}
}
if (!vehiclePoly.DoesIntersect(ls) && !pathTooClose)
{
Obstacle obs = CreatePerimeterObstacle(ls);
perimeterObstacles.Add(obs);
OperationalObstacle uiobs = new OperationalObstacle();
uiobs.age = obs.age;
uiobs.heading = 0;
uiobs.headingValid = false;
uiobs.ignored = false;
uiobs.obstacleClass = obs.obstacleClass;
uiobs.poly = obs.obstaclePolygon;
operationalObstacles.Add(uiobs);
}
}
}
Services.UIService.PushObstacles(operationalObstacles.ToArray(), curTimestamp, "perimeter obstacles", true);
return(perimeterObstacles);
}
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());
}
}
private static double FindBestCurvature(double prevCurvature, Coordinates relativeGoalPoint, List <Polygon> perimeterPolygons)
{
CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer();
// get a list of obstacles
ObstacleCollection obstacles = Services.ObstaclePipeline.GetProcessedObstacles(curTimestamp, UrbanChallenge.Behaviors.SAUDILevel.None);
List <Polygon> obstaclePolygons = new List <Polygon>();
foreach (Obstacle obs in obstacles.obstacles)
{
obstaclePolygons.Add(obs.cspacePolygon);
}
obstaclePolygons.AddRange(perimeterPolygons);
List <ArcResults> arcs = new List <ArcResults>();
double maxUtility = double.MinValue;
ArcResults selectedArc = null;
// recalculate weights
double totalWeights = obstacle_weight + hysteresis_weight + straight_weight + goal_weight;
double obstacleWeight = obstacle_weight / totalWeights;
double hysteresisWeight = hysteresis_weight / totalWeights;
double straightWeight = straight_weight / totalWeights;
double goalWeight = goal_weight / totalWeights;
int start = num_arcs / 2;
double curvatureStep = max_curvature / start;
for (int i = -start; i <= start; i++)
{
double curvature = i * curvatureStep;
double collisionDist, clearanceDist, collisionUtility;
bool vetoed;
EvaluateObstacleUtility(curvature, 20, obstaclePolygons, out collisionDist, out clearanceDist, out collisionUtility, out vetoed);
double hystersisUtility = EvaluateHysteresisUtility(curvature, prevCurvature);
double straightUtility = EvaluateStraightUtility(curvature);
double goalUtility = EvaluateGoalUtility(curvature, relativeGoalPoint);
double totalUtility = collisionUtility * obstacleWeight + hystersisUtility * hysteresisWeight + straightUtility * straightWeight +
goalUtility * goalWeight;
ArcResults result = new ArcResults();
result.curvature = curvature;
result.vetoed = vetoed;
result.totalUtility = totalUtility;
result.obstacleHitDistance = collisionDist;
result.obstacleClearanceDistance = clearanceDist;
result.obstacleUtility = collisionUtility;
result.hysteresisUtility = hystersisUtility;
result.straightUtility = straightUtility;
result.goalUtility = goalUtility;
arcs.Add(result);
if (!vetoed && totalUtility > maxUtility)
{
maxUtility = totalUtility;
selectedArc = result;
}
}
ArcVotingResults results = new ArcVotingResults();
results.arcResults = arcs;
results.selectedArc = selectedArc;
Services.Dataset.ItemAs <ArcVotingResults>("arc voting results").Add(results, LocalCarTimeProvider.LocalNow);
if (selectedArc == null)
{
return(double.NaN);
}
else
{
return(selectedArc.curvature);
}
}
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);
}