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