/// <summary> /// Determines proper speed commands given we want to stop at a certain waypoint /// </summary> /// <param name="waypoint"></param> /// <param name="lane"></param> /// <param name="position"></param> /// <param name="enCovariance"></param> /// <param name="stopSpeed"></param> /// <param name="stopDistance"></param> public void StoppingParams(Coordinates coordinate, IFQMPlanable lane, Coordinates position, double[] enCovariance, out double stopSpeed, out double stopDistance) { // get dist to waypoint stopDistance = lane.DistanceBetween(position, coordinate); // subtract distance based upon type to help calculate speed double stopSpeedDistance = stopDistance - CoreCommon.OperationalStopDistance; // check if we are positive distance away if (stopSpeedDistance >= 0) { // segment max speed double segmentMaxSpeed = lane.CurrentMaximumSpeed(position); // get speed using constant acceleration stopSpeed = SpeedTools.GenerateSpeed(stopSpeedDistance, CoreCommon.OperationalStopSpeed, segmentMaxSpeed); } else { // inside stop dist stopSpeed = (stopDistance / CoreCommon.OperationalStopDistance) * CoreCommon.OperationalStopSpeed; stopSpeed = stopSpeed < 0 ? 0.0 : stopSpeed; } }
/// <summary> /// Determines proper speed commands given we want to stop at a certain waypoint /// </summary> /// <param name="waypoint"></param> /// <param name="lane"></param> /// <param name="position"></param> /// <param name="enCovariance"></param> /// <param name="stopSpeed"></param> /// <param name="stopDistance"></param> public void StoppingParams(ArbiterWaypoint waypoint, ArbiterLane lane, Coordinates position, double extraDistance, out double stopSpeed, out double stopDistance) { // get dist to waypoint stopDistance = lane.DistanceBetween(position, waypoint.Position) - extraDistance; // speed tools stopSpeed = SpeedTools.GenerateSpeed(stopDistance, CoreCommon.OperationalStopSpeed, 2.24); }
/// <summary> /// Gets speed we are supposed to be at given we are stopping at some distance /// </summary> /// <param name="distanceToDpoi"></param> /// <returns></returns> private double StoppingParameters(double distanceToDpoi, double finalAreaSpeed) { // subtract distance based upon type to help calculate speed double stopSpeedDistance = distanceToDpoi - CoreCommon.OperationalStopDistance; // check if we are positive distance away if (stopSpeedDistance >= 0) { // segment max speed double segmentMaxSpeed = Math.Min(turn.MaximumDefaultSpeed, finalAreaSpeed); // distance to stop from max v given desired acceleration //double stopEnvelopeLength = (Math.Pow(CoreCommon.OperationalStopSpeed, 2) - Math.Pow(segmentMaxSpeed, 2)) / (2.0 * -CoreCommon.DesiredAcceleration); // check if we are within profile if (stopSpeedDistance > 0.0) { // shifted speed //double shiftedDistanceToEnd = stopEnvelopeLength - stopSpeedDistance; // get speed along profile double speedGen = SpeedTools.GenerateSpeed(stopSpeedDistance, CoreCommon.OperationalStopSpeed, segmentMaxSpeed); return(speedGen); //return CoreCommon.OperationalStopSpeed + (shiftedMaxSpeed - ((stopEnvelopeLength - shiftedDistanceToEnd)/stopEnvelopeLength * shiftedMaxSpeed)); } else { return(segmentMaxSpeed); } } else { // inside stop dist spool to 0 double stopSpeed = (distanceToDpoi / CoreCommon.OperationalStopDistance) * CoreCommon.OperationalStopSpeed; stopSpeed = stopSpeed < 0 ? 0.0 : stopSpeed; return(stopSpeed); } }
/// <summary> /// Figure out standard control to follow /// </summary> /// <param name="lane"></param> /// <param name="state"></param> /// <returns></returns> public ForwardVehicleTrackingControl GetControl(ArbiterLane lane, VehicleState state) { // check exists a vehicle to track if (this.CurrentVehicle != null) { // get the maximum velocity of the segment we're closest to double segV = lane.CurrentMaximumSpeed(state.Front); double segMaxV = segV; // minimum distance double xAbsMin = TahoeParams.VL * 2.0; // retrieve the tracked vehicle's scalar absolute speed double vTarget = this.CurrentVehicle.StateMonitor.Observed.speedValid ? this.CurrentVehicle.Speed : lane.Way.Segment.SpeedLimits.MaximumSpeed; // check if vehicle is moving away from us VehicleDirectionAlongPath vdap = CurrentVehicle.VehicleDirectionAlong(lane); // get the good distance behind the target, is xAbsMin if vehicle velocity is small enough double xGood = xAbsMin + (1.5 * (2 * TahoeParams.VL) * vTarget); // get our current separation double xSeparation = this.currentDistance; // determine the envelope to reason about the vehicle, that is, to slow from vMax to vehicle speed double xEnvelope = (Math.Pow(vTarget, 2.0) - Math.Pow(segMaxV, 2.0)) / (2.0 * -0.5); // the distance to the good double xDistanceToGood; // determine the velocity to follow the vehicle ahead double vFollowing; // check if we are basically stopped in the right place behind another vehicle if (vTarget < 1 && Math.Abs(xSeparation - xGood) < 1) { // stop vFollowing = 0; // good distance xDistanceToGood = 0; } // check if we are within the minimum distance else if (xSeparation <= xGood) { // stop vFollowing = 0; // good distance xDistanceToGood = 0; } // determine if our separation is less than the good distance but not inside minimum else if (xAbsMin < xSeparation && xSeparation < xGood) { // get the distance we are from xGood to xMin double xAlong = xSeparation - xAbsMin; // get the total distance from xGood to xMin double xGoodToMin = xGood - xAbsMin; // slow to 0 by min vFollowing = (((xGoodToMin - xAlong) / xGoodToMin) * (-vTarget)) + vTarget; // good distance xDistanceToGood = 0; } // otherwise xSeparation > xEnvelope else { // good distance xDistanceToGood = xSeparation - xGood; // get differences in max and target velocities vFollowing = SpeedTools.GenerateSpeed(xDistanceToGood, vTarget, segMaxV); } // return return(new ForwardVehicleTrackingControl(true, false, vFollowing, xSeparation, xDistanceToGood, vTarget, xAbsMin, xGood, true)); } else { // return return(new ForwardVehicleTrackingControl(false, false, Double.MaxValue, Double.MaxValue, Double.MaxValue, Double.MaxValue, 0.0, Double.MaxValue, false)); } }
/// <summary> /// Behavior given we stay in the current lane /// </summary> /// <param name="lane"></param> /// <param name="state"></param> /// <param name="downstreamPoint"></param> /// <returns></returns> public TravelingParameters Primary(IFQMPlanable lane, VehicleState state, RoadPlan roadPlan, List <ITacticalBlockage> blockages, List <ArbiterWaypoint> ignorable, bool log) { // possible parameterizations List <TravelingParameters> tps = new List <TravelingParameters>(); #region Lane Major Parameterizations with Current Lane Goal Params, If Best Goal Exists in Current Lane // check if the best goal is in the current lane ArbiterWaypoint lanePoint = null; if (lane.AreaComponents.Contains(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Lane)) { lanePoint = roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest; } // get the next thing we need to stop at no matter what and parameters for stopping at it ArbiterWaypoint laneNavStop; double laneNavStopSpeed; double laneNavStopDistance; StopType laneNavStopType; this.NextNavigationalStop(lane, lanePoint, state.Front, state.ENCovariance, ignorable, out laneNavStopSpeed, out laneNavStopDistance, out laneNavStopType, out laneNavStop); // create parameterization of the stop TravelingParameters laneNavParams = this.NavStopParameterization(lane, roadPlan, laneNavStopSpeed, laneNavStopDistance, laneNavStop, laneNavStopType, state); this.navigationParameters = laneNavParams; this.laneParameters = laneNavParams; tps.Add(laneNavParams); #region Log if (log) { // add to current parames to arbiter information CoreCommon.CurrentInformation.FQMBehavior = laneNavParams.Behavior.ToShortString(); CoreCommon.CurrentInformation.FQMBehaviorInfo = laneNavParams.Behavior.ShortBehaviorInformation(); CoreCommon.CurrentInformation.FQMSpeedCommand = laneNavParams.Behavior.SpeedCommandString(); CoreCommon.CurrentInformation.FQMDistance = laneNavParams.DistanceToGo.ToString("F6"); CoreCommon.CurrentInformation.FQMSpeed = laneNavParams.RecommendedSpeed.ToString("F6"); CoreCommon.CurrentInformation.FQMState = laneNavParams.NextState.ShortDescription(); CoreCommon.CurrentInformation.FQMStateInfo = laneNavParams.NextState.StateInformation(); CoreCommon.CurrentInformation.FQMStopType = laneNavStopType.ToString(); CoreCommon.CurrentInformation.FQMWaypoint = laneNavStop.ToString(); CoreCommon.CurrentInformation.FQMSegmentSpeedLimit = lane.CurrentMaximumSpeed(state.Position).ToString("F1"); } #endregion #endregion #region Forward Vehicle Parameterization // forward vehicle update this.ForwardVehicle.Update(lane, state); // clear current params this.followingParameters = null; // check not in a sparse area bool sparseArea = lane is ArbiterLane ? ((ArbiterLane)lane).GetClosestPartition(state.Front).Type == PartitionType.Sparse : ((SupraLane)lane).ClosestComponent(state.Front) == SLComponentType.Initial && ((SupraLane)lane).Initial.GetClosestPartition(state.Front).Type == PartitionType.Sparse; // exists forward vehicle if (!sparseArea && this.ForwardVehicle.ShouldUseForwardTracker) { // get forward vehicle params, set lane decorators TravelingParameters vehicleParams = this.ForwardVehicle.Follow(lane, state, ignorable); vehicleParams.Behavior.Decorators.AddRange(this.laneParameters.Decorators); this.FollowingParameters = vehicleParams; #region Log if (log) { // add to current parames to arbiter information CoreCommon.CurrentInformation.FVTBehavior = vehicleParams.Behavior.ToShortString(); CoreCommon.CurrentInformation.FVTSpeed = this.ForwardVehicle.FollowingParameters.RecommendedSpeed.ToString("F3"); CoreCommon.CurrentInformation.FVTSpeedCommand = vehicleParams.Behavior.SpeedCommandString(); CoreCommon.CurrentInformation.FVTDistance = vehicleParams.DistanceToGo.ToString("F2"); CoreCommon.CurrentInformation.FVTState = vehicleParams.NextState.ShortDescription(); CoreCommon.CurrentInformation.FVTStateInfo = vehicleParams.NextState.StateInformation(); // set xSeparation CoreCommon.CurrentInformation.FVTXSeparation = this.ForwardVehicle.ForwardControl.xSeparation.ToString("F0"); } #endregion // check if we're stopped behind fv, allow wait timer if true, stop wait timer if not behind fv bool forwardVehicleStopped = this.ForwardVehicle.CurrentVehicle.IsStopped; bool forwardSeparationGood = this.ForwardVehicle.ForwardControl.xSeparation < TahoeParams.VL * 2.5; bool wereStopped = CoreCommon.Communications.GetVehicleSpeed().Value < 0.1; bool forwardDistanceToGo = vehicleParams.DistanceToGo < 3.5; if (forwardVehicleStopped && forwardSeparationGood && wereStopped && forwardDistanceToGo) { this.ForwardVehicle.StoppedBehindForwardVehicle = true; } else { this.ForwardVehicle.StoppedBehindForwardVehicle = false; this.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Stop(); this.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Reset(); } // add vehicle param tps.Add(vehicleParams); } else { // no forward vehicle this.followingParameters = null; this.ForwardVehicle.StoppedBehindForwardVehicle = false; } #endregion #region Sparse Waypoint Parameterization // check for sparse waypoints downstream bool sparseDownstream; bool sparseNow; double sparseDistance; lane.SparseDetermination(state.Front, out sparseDownstream, out sparseNow, out sparseDistance); // check if sparse areas downstream if (sparseDownstream) { // set the distance to the sparse area if (sparseNow) { sparseDistance = 0.0; } // get speed double speed = SpeedTools.GenerateSpeed(sparseDistance, 2.24, lane.CurrentMaximumSpeed(state.Front)); // maneuver Maneuver m = new Maneuver(); bool usingSpeed = true; SpeedCommand sc = new ScalarSpeedCommand(speed); #region Parameterize Given Speed Command // check if lane if (lane is ArbiterLane) { // get lane ArbiterLane al = (ArbiterLane)lane; // default behavior Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(speed), new List <int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true)); b.Decorators = this.laneParameters.Decorators; // standard behavior is fine for maneuver m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), this.laneParameters.Decorators, state.Timestamp); } // check if supra lane else if (lane is SupraLane) { // get lane SupraLane sl = (SupraLane)lane; // get sl state StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState; // get default beheavior Behavior b = sisls.GetBehavior(new ScalarSpeedCommand(speed), state.Front, new List <int>()); b.Decorators = this.laneParameters.Decorators; // standard behavior is fine for maneuver m = new Maneuver(b, sisls, this.laneParameters.Decorators, state.Timestamp); } #endregion #region Parameterize // create new params TravelingParameters tp = new TravelingParameters(); tp.Behavior = m.PrimaryBehavior; tp.Decorators = this.laneParameters.Decorators; tp.DistanceToGo = Double.MaxValue; tp.NextState = m.PrimaryState; tp.RecommendedSpeed = speed; tp.Type = TravellingType.Navigation; tp.UsingSpeed = usingSpeed; tp.SpeedCommand = sc; tp.VehiclesToIgnore = new List <int>(); // return navigation params tps.Add(tp); #endregion } #endregion // sort params by most urgent tps.Sort(); // set current params this.currentParameters = tps[0]; // get behavior to check add vehicles to ignore if (this.currentParameters.Behavior is StayInLaneBehavior) { ((StayInLaneBehavior)this.currentParameters.Behavior).IgnorableObstacles = this.ForwardVehicle.VehiclesToIgnore; } // out of navigation, blockages, and vehicle following determine the actual primary parameters for this lane return(tps[0]); }
/// <summary> /// Figure out standard control to follow /// </summary> /// <param name="lane"></param> /// <param name="state"></param> /// <returns></returns> public ForwardVehicleTrackingControl GetControl(IFQMPlanable lane, VehicleState state, List <ArbiterWaypoint> ignorable) { // check exists a vehicle to track if (this.CurrentVehicle != null) { // get the maximum velocity of the segment we're closest to double segV = lane.CurrentMaximumSpeed(state.Front); double segMaxV = segV; // flag if the forward vehicles is in a stop waypoint type safety zone bool safetyZone = false; ArbiterWaypoint nextStop = lane.GetNext(state.Front, WaypointType.Stop, ignorable); double distance = nextStop != null?lane.DistanceBetween(state.Front, nextStop.Position) : Double.MaxValue; // check if no stop exists or distance > 30 if (nextStop == null || distance > 30 || distance < 0) { // there is no next stop or the forward vehcile is not in a safety zone safetyZone = false; } // otherwise the tracked vehicle is in a safety zone else { // set the forward vehicle as being in a safety zone safetyZone = true; } // minimum distance double xAbsMin; // if we're in a safety zone if (safetyZone) { // set minimum to 1 times the tahoe's vehicle length xAbsMin = TahoeParams.VL * 1.5; } // otherwise we're not in a safety zone else { // set minimum to 2 times the tahoe's vehicle length xAbsMin = TahoeParams.VL * 2.0; } // retrieve the tracked vehicle's scalar absolute speed double vTarget = CurrentVehicle.StateMonitor.Observed.isStopped ? 0.0 : this.CurrentVehicle.Speed; // check if vehicle is moving away from us VehicleDirectionAlongPath vdap = CurrentVehicle.VehicleDirectionAlong(lane); // if moving perpendic, set speed to 0.0 if (vdap == VehicleDirectionAlongPath.Perpendicular) { vTarget = 0.0; } #region Forward // can use normal tracker if vehicle not oncoming if (true) //vdap != VehicleDirectionAlongPath.Reverse) { // get the good distance behind the target, is xAbsMin if vehicle velocity is small enough double xGood = xAbsMin + (1.5 * (TahoeParams.VL / 4.4704) * vTarget); // get our current separation double xSeparation = this.currentDistance; // determine the envelope to reason about the vehicle, that is, to slow from vMax to vehicle speed double xEnvelope = (Math.Pow(vTarget, 2.0) - Math.Pow(segMaxV, 2.0)) / (2.0 * -0.5); // the distance to the good double xDistanceToGood; // determine the velocity to follow the vehicle ahead double vFollowing; // check if we are basically stopped in the right place behind another vehicle if (vTarget < 1 && Math.Abs(xSeparation - xGood) < 1) { // stop vFollowing = 0; // good distance xDistanceToGood = 0; } // check if we are within the minimum distance else if (xSeparation <= xAbsMin) { // stop vFollowing = 0; // good distance xDistanceToGood = 0; } // determine if our separation is less than the good distance but not inside minimum else if (xAbsMin < xSeparation && xSeparation < xGood) { // get the distance we are from xMin double xAlong = xSeparation - xAbsMin; // get the total distance from xGood to xMin double xGoodToMin = xGood - xAbsMin; // slow to 0 by min vFollowing = (((xGoodToMin - xAlong) / xGoodToMin) * (-vTarget)) + vTarget; // good distance xDistanceToGood = 0; } // our separation is greater than the good distance else { // good distance xDistanceToGood = xSeparation - xGood; // get differences in max and target velocities vFollowing = SpeedTools.GenerateSpeed(xDistanceToGood, vTarget, segMaxV); } // return return(new ForwardVehicleTrackingControl(true, safetyZone, vFollowing, xSeparation, xDistanceToGood, vTarget, xAbsMin, xGood, false)); } #endregion #region Oncoming /* * else * { * // determine the distance for the other vehicle to stop * double xTargetEnvelope = -Math.Pow(vTarget, 2.0) / (2.0 * -0.5); * * // determine the distance for our vehicle to stop * double xOurEnvelope = -Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2.0) / (2.0 * -0.5); * * // get the good distance behind the target, is xAbsMin if vehicle velocity is small enough * double xGood = xAbsMin + (1.5 * (TahoeParams.VL / 4.4704) * vTarget) + xOurEnvelope + xTargetEnvelope; * * // get our current separation * double xSeparation = this.currentDistance; * * // determine the envelope for us to slow to 0 by the good distance * double xEnvelope = -Math.Pow(segMaxV, 2.0) / (2.0 * -0.5); * * // the distance to the good * double xDistanceToGood; * * // determine the velocity to follow the vehicle ahead * double vFollowing; * * // check if we are basically stopped in the right place behind another vehicle * if (vTarget < 1 && Math.Abs(xSeparation - xGood) < 1) * { * // stop * vFollowing = 0; * * // good distance * xDistanceToGood = 0; * } * // check if we are within the minimum distance * else if (xSeparation <= xGood) * { * // stop * vFollowing = 0; * * // good distance * xDistanceToGood = 0; * } * // our separation is greater than the good distance but within the envelope * else if (xGood <= xSeparation && xSeparation <= xEnvelope + xGood) * { * // get differences in max and target velocities * double vDifference = segMaxV; * * // get the distance we are along the speed envolope from xGood * double xAlong = xEnvelope - (xSeparation - xGood); * * // slow to vTarget by good * vFollowing = (((xEnvelope - xAlong) / xEnvelope) * (vDifference)); * * // good distance * xDistanceToGood = xSeparation - xGood; * } * // otherwise xSeparation > xEnvelope * else * { * // can go max speed * vFollowing = segMaxV; * * // good distance * xDistanceToGood = xSeparation - xGood; * } * * // return * return new ForwardVehicleTrackingControl(true, safetyZone, vFollowing, xSeparation, * xDistanceToGood, vTarget, xAbsMin, xGood, true); * } * */ #endregion } else { // return return(new ForwardVehicleTrackingControl(false, false, Double.MaxValue, Double.MaxValue, Double.MaxValue, Double.MaxValue, 0.0, Double.MaxValue, false)); } }
/// <summary> /// Gets primary maneuver given our position and the turn we are traveling upon /// </summary> /// <param name="vehicleState"></param> /// <returns></returns> public Maneuver PrimaryManeuver(VehicleState vehicleState, List <ITacticalBlockage> blockages, TurnState turnState) { #region Check are planning over the correct turn if (CoreCommon.CorePlanningState is TurnState) { TurnState ts = (TurnState)CoreCommon.CorePlanningState; if (this.turn == null || !this.turn.Equals(ts.Interconnect)) { this.turn = ts.Interconnect; this.forwardMonitor = new TurnForwardMonitor(ts.Interconnect, null); } else if (this.forwardMonitor.turn == null || !this.forwardMonitor.turn.Equals(ts.Interconnect)) { this.forwardMonitor = new TurnForwardMonitor(ts.Interconnect, null); } } #endregion #region Blockages // check blockages if (blockages != null && blockages.Count > 0 && blockages[0] is TurnBlockage) { // create the blockage state EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState); // check not at highest level already if (turnState.Saudi != SAUDILevel.L1 || turnState.UseTurnBounds) { // check not from a dynamicly moving vehicle if (blockages[0].BlockageReport.BlockageType != BlockageType.Dynamic || (TacticalDirector.ValidVehicles.ContainsKey(blockages[0].BlockageReport.TrackID) && TacticalDirector.ValidVehicles[blockages[0].BlockageReport.TrackID].IsStopped)) { // go to a blockage handling tactical return(new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp)); } else { ArbiterOutput.Output("Turn blockage reported for moving vehicle, ignoring"); } } else { ArbiterOutput.Output("Turn blockage, but recovery escalation already at highest state, ignoring report"); } } #endregion #region Intersection Check if (!this.CanGo(vehicleState)) { if (turn.FinalGeneric is ArbiterWaypoint) { TravelingParameters tp = this.GetParameters(0.0, 0.0, (ArbiterWaypoint)turn.FinalGeneric, vehicleState, false); return(new Maneuver(tp.Behavior, CoreCommon.CorePlanningState, tp.NextState.DefaultStateDecorators, vehicleState.Timestamp)); } else { // get turn params LinePath finalPath; LineList leftLL; LineList rightLL; IntersectionToolkit.ZoneTurnInfo(this.turn, (ArbiterPerimeterWaypoint)this.turn.FinalGeneric, out finalPath, out leftLL, out rightLL); // hold brake IState nextState = new TurnState(this.turn, this.turn.TurnDirection, null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(0.0)); TurnBehavior b = new TurnBehavior(null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(0.0), this.turn.InterconnectId); return(new Maneuver(b, CoreCommon.CorePlanningState, nextState.DefaultStateDecorators, vehicleState.Timestamp)); } } #endregion #region Final is Lane Waypoint if (turn.FinalGeneric is ArbiterWaypoint) { // final point ArbiterWaypoint final = (ArbiterWaypoint)turn.FinalGeneric; // plan down entry lane RoadPlan rp = navigation.PlanNavigableArea(final.Lane, final.Position, CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId], new List <ArbiterWaypoint>()); // point of interest downstream DownstreamPointOfInterest dpoi = rp.BestPlan.laneWaypointOfInterest; // get path this represents List <Coordinates> pathCoordinates = new List <Coordinates>(); pathCoordinates.Add(vehicleState.Position); foreach (ArbiterWaypoint aw in final.Lane.WaypointsInclusive(final, final.Lane.WaypointList[final.Lane.WaypointList.Count - 1])) { pathCoordinates.Add(aw.Position); } LinePath lp = new LinePath(pathCoordinates); // list of all parameterizations List <TravelingParameters> parameterizations = new List <TravelingParameters>(); // get lane navigation parameterization TravelingParameters navigationParameters = this.NavigationParameterization(vehicleState, dpoi, final, lp); parameterizations.Add(navigationParameters); // update forward tracker and get vehicle parameterizations if forward vehicle exists this.forwardMonitor.Update(vehicleState, final, lp); if (this.forwardMonitor.ShouldUseForwardTracker()) { // get vehicle parameterization TravelingParameters vehicleParameters = this.VehicleParameterization(vehicleState, lp, final); parameterizations.Add(vehicleParameters); } // sort and return funal parameterizations.Sort(); // get the final behavior TurnBehavior tb = (TurnBehavior)parameterizations[0].Behavior; // get vehicles to ignore tb.VehiclesToIgnore = this.forwardMonitor.VehiclesToIgnore; // add persistent information about saudi level if (turnState.Saudi == SAUDILevel.L1) { tb.Decorators = new List <BehaviorDecorator>(tb.Decorators.ToArray()); tb.Decorators.Add(new ShutUpAndDoItDecorator(SAUDILevel.L1)); } // add persistent information about turn bounds if (!turnState.UseTurnBounds) { tb.LeftBound = null; tb.RightBound = null; } // return the behavior return(new Maneuver(tb, CoreCommon.CorePlanningState, tb.Decorators, vehicleState.Timestamp)); } #endregion #region Final is Zone Waypoint else if (turn.FinalGeneric is ArbiterPerimeterWaypoint) { // get inteconnect path Coordinates entryVec = ((ArbiterPerimeterWaypoint)turn.FinalGeneric).Perimeter.PerimeterPolygon.BoundingCircle.center - turn.FinalGeneric.Position; entryVec = entryVec.Normalize(TahoeParams.VL / 2.0); LinePath ip = new LinePath(new Coordinates[] { turn.InitialGeneric.Position, turn.FinalGeneric.Position, entryVec + this.turn.FinalGeneric.Position }); // get distance from end double d = ip.DistanceBetween( ip.GetClosestPoint(vehicleState.Front), ip.EndPoint); // get speed command SpeedCommand sc = null; if (d < TahoeParams.VL) { sc = new StopAtDistSpeedCommand(d); } else { sc = new ScalarSpeedCommand(SpeedTools.GenerateSpeed(d - TahoeParams.VL, 1.7, turn.MaximumDefaultSpeed)); } // final perimeter waypoint ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)this.turn.FinalGeneric; // get turn params LinePath finalPath; LineList leftLL; LineList rightLL; IntersectionToolkit.ZoneTurnInfo(this.turn, (ArbiterPerimeterWaypoint)this.turn.FinalGeneric, out finalPath, out leftLL, out rightLL); // hold brake IState nextState = new TurnState(this.turn, this.turn.TurnDirection, null, finalPath, leftLL, rightLL, sc); TurnBehavior tb = new TurnBehavior(null, finalPath, leftLL, rightLL, sc, null, new List <int>(), this.turn.InterconnectId); // add persistent information about saudi level if (turnState.Saudi == SAUDILevel.L1) { tb.Decorators = new List <BehaviorDecorator>(tb.Decorators.ToArray()); tb.Decorators.Add(new ShutUpAndDoItDecorator(SAUDILevel.L1)); } // add persistent information about turn bounds if (!turnState.UseTurnBounds) { tb.LeftBound = null; tb.RightBound = null; } // return maneuver return(new Maneuver(tb, CoreCommon.CorePlanningState, tb.Decorators, vehicleState.Timestamp)); } #endregion #region Unknown else { throw new Exception("unrecognized type: " + turn.FinalGeneric.ToString()); } #endregion }