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; } }