public ChangeLaneBehavior(ArbiterLaneId startLane, ArbiterLaneId targetLane, bool changeLeft, double maxDist, SpeedCommand speedCommand,
IEnumerable <int> ignorableObstacles, LinePath backupStartLanePath, LinePath backupTargetLanePath, double startWidth, double targetWidth,
int startingNumLanesLeft, int startingNumLanesRight)
{
this.startLane = startLane;
this.targetLane = targetLane;
this.maxDist = maxDist;
this.speedCommand = speedCommand;
this.changeLeft = changeLeft;
this.backupStartLanePath = backupStartLanePath;
this.backupTargetLanePath = backupTargetLanePath;
this.startWidth = startWidth;
this.targetWidth = targetWidth;
this.startingNumLanesLeft = startingNumLanesLeft;
this.startingNumLanesRight = startingNumLanesRight;
if (ignorableObstacles != null)
{
this.ignorableObstacles = new List <int>(ignorableObstacles);
}
else
{
this.ignorableObstacles = new List <int>();
}
}
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;
}
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 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 UTurnBehavior(Polygon boundary, LinePath endingPath, ArbiterLaneId endingLane, SpeedCommand endingSpeedCommand)
{
this.boundary = boundary;
this.endingPath = endingPath;
this.endingLane = endingLane;
this.endingSpeedCommand = endingSpeedCommand;
this.stayOutPolygons = new List <Polygon>();
}
public UTurnBehavior(Polygon boundary, LinePath endingPath, ArbiterLaneId endingLane, SpeedCommand endingSpeedCommand, List <Polygon> stayOutPolygons)
{
this.boundary = boundary;
this.endingPath = endingPath;
this.endingLane = endingLane;
this.endingSpeedCommand = endingSpeedCommand;
this.stayOutPolygons = stayOutPolygons;
this.stopOnEndingPath = true;
}
public TurnBehavior(ArbiterLaneId targetLane, LinePath targetLanePath, LineList leftBound, LineList rightBound, SpeedCommand speedCommand, ArbiterInterconnectId interconnectId)
{
this.targetLane = targetLane;
this.targetLanePath = targetLanePath;
this.leftBound = leftBound;
this.rightBound = rightBound;
this.speedCommand = speedCommand;
this.ignorableObstacles = new List <int>();
this.interconnectId = interconnectId;
this.VehiclesToIgnore = new List <int>();
this.decorators = new List <BehaviorDecorator>();
}
public StayInLaneBehavior(ArbiterLaneId targetLane, SpeedCommand speedCommand, IEnumerable <int> ignorableObstacles)
{
this.targetLane = targetLane;
this.speedCommand = speedCommand;
if (ignorableObstacles != null)
{
this.ignorableObstacles = new List <int>(ignorableObstacles);
}
else
{
this.ignorableObstacles = new List <int>();
}
}
public TurnBehavior(ArbiterLaneId targetLane, LinePath targetLanePath, LineList leftBound, LineList rightBound, SpeedCommand speedCommand, Polygon intersectionPolygon, IEnumerable <int> ignorableObstacles, ArbiterInterconnectId interconnectId)
{
this.targetLane = targetLane;
this.targetLanePath = targetLanePath;
this.leftBound = leftBound;
this.rightBound = rightBound;
this.speedCommand = speedCommand;
this.intersectionPolygon = intersectionPolygon;
this.ignorableObstacles = ignorableObstacles != null ? new List <int>(ignorableObstacles) : new List <int>();
this.interconnectId = interconnectId;
this.VehiclesToIgnore = new List <int>();
this.decorators = new List <BehaviorDecorator>();
}
public StayInLaneBehavior(ArbiterLaneId targetLane, SpeedCommand speedCommand, IEnumerable <int> ignorableObstacles, LinePath backupPath, double laneWidth, int numLanesLeft, int numLanesRight)
{
this.targetLane = targetLane;
this.speedCommand = speedCommand;
if (ignorableObstacles != null)
{
this.ignorableObstacles = new List <int>(ignorableObstacles);
}
else
{
this.ignorableObstacles = new List <int>();
}
this.backupPath = backupPath;
this.laneWidth = laneWidth;
this.numLanesLeft = numLanesLeft;
this.numLanesRight = numLanesRight;
}
public ChangeLaneBehavior(ArbiterLaneId startLane, ArbiterLaneId targetLane, bool changeLeft, double maxDist, SpeedCommand speedCommand,
IEnumerable <int> ignorableObstacles)
{
this.startLane = startLane;
this.targetLane = targetLane;
this.maxDist = maxDist;
this.speedCommand = speedCommand;
this.changeLeft = changeLeft;
if (ignorableObstacles != null)
{
this.ignorableObstacles = new List <int>(ignorableObstacles);
}
else
{
this.ignorableObstacles = new List <int>();
}
}
/// <summary>
/// Distance to the next stop line
/// </summary>
/// <param name="simVehicleState"></param>
/// <returns></returns>
public double?DistanceToNextStop(SimVehicleState simVehicleState)
{
if (this.RoadNetwork == null)
{
return(null);
}
else
{
IAreaSubtypeId iasi = this.GetAreaId(simVehicleState);
if (iasi == null)
{
return(null);
}
else if (iasi is ArbiterPerimeterId)
{
return(null);
}
else
{
ArbiterLaneId ali = (ArbiterLaneId)iasi;
// get closest
ArbiterLanePartition alp = this.RoadNetwork.ArbiterSegments[ali.SegmentId].Lanes[ali].GetClosestPartition(simVehicleState.Position);
ArbiterWaypoint waypoint;
double distance;
RoadNetworkTools.NextStop(alp.Lane, alp, simVehicleState.Position, out waypoint, out distance);
if (waypoint == null)
{
return(null);
}
else
{
return(distance);
}
}
}
}
/*public SupraLaneBehavior(
* ArbiterLaneId startingLaneId, LinePath startingLanePath, double startingLaneWidth, int startingNumLanesLeft, int startingNumLanesRight,
* ArbiterLaneId endingLaneId, LinePath endingLanePath, double endingLaneWidth, int endingNumLanesLeft, int endingNumLanesRight,
* SpeedCommand speedCommand, List<int> ignorableObstacles) {
*
* this.startingLaneId = startingLaneId;
* this.startingLanePath = startingLanePath;
* this.startingLaneWidth = startingLaneWidth;
* this.startingNumLanesLeft = startingNumLanesLeft;
* this.startingNumLanesRight = startingNumLanesRight;
*
* this.endingLaneId = endingLaneId;
* this.endingLanePath = endingLanePath;
* this.endingLaneWidth = endingLaneWidth;
* this.endingNumLanesLeft = endingNumLanesLeft;
* this.endingNumLanesRight = endingNumLanesRight;
*
* this.speedCommand = speedCommand;
* this.ignorableObstacles = ignorableObstacles;
* }*/
public SupraLaneBehavior(
ArbiterLaneId startingLaneId, LinePath startingLanePath, double startingLaneWidth, int startingNumLanesLeft, int startingNumLanesRight,
ArbiterLaneId endingLaneId, LinePath endingLanePath, double endingLaneWidth, int endingNumLanesLeft, int endingNumLanesRight,
SpeedCommand speedCommand, List <int> ignorableObstacles, Polygon intersectionPolygon)
{
this.startingLaneId = startingLaneId;
this.startingLanePath = startingLanePath;
this.startingLaneWidth = startingLaneWidth;
this.startingNumLanesLeft = startingNumLanesLeft;
this.startingNumLanesRight = startingNumLanesRight;
this.endingLaneId = endingLaneId;
this.endingLanePath = endingLanePath;
this.endingLaneWidth = endingLaneWidth;
this.endingNumLanesLeft = endingNumLanesLeft;
this.endingNumLanesRight = endingNumLanesRight;
this.intersectionPolygon = intersectionPolygon;
this.speedCommand = speedCommand;
this.ignorableObstacles = ignorableObstacles;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initial"></param>
/// <param name="target"></param>
public InternalState(ArbiterLaneId initial, ArbiterLaneId target, Probability confidence)
{
this.Initial = initial;
this.Target = target;
this.Confidence = confidence;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initial"></param>
/// <param name="target"></param>
public InternalState(ArbiterLaneId initial, ArbiterLaneId target)
{
this.Initial = initial;
this.Target = target;
this.Confidence = new Probability(0.8, 0.2);
}
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;
}
}
