/// <summary>
/// Gets parameters for following forward vehicle
/// </summary>
/// <param name="vehicleState"></param>
/// <param name="fullPath"></param>
/// <param name="followingSpeed"></param>
/// <param name="distanceToGood"></param>
public void Follow(VehicleState state, LinePath fullPath, ArbiterLane lane, ArbiterInterconnect turn,
out double followingSpeed, out double distanceToGood, out double xSep)
{
// get the maximum velocity of the segment we're closest to
double segV = Math.Min(lane.CurrentMaximumSpeed(state.Front), turn.MaximumDefaultSpeed);
double segMaxV = segV;
// minimum distance
double xAbsMin = TahoeParams.VL * 1.5;
// retrieve the tracked vehicle's scalar absolute speed
double vTarget = CurrentVehicle.StateMonitor.Observed.isStopped ? 0.0 : this.CurrentVehicle.Speed;
// 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 = state.Front.DistanceTo(this.CurrentVehicle.ClosestPosition);//lane.DistanceBetween(state.Front, this.CurrentVehicle.ClosestPosition);
xSep = xSeparation;
// 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 but within the envelope
else if (xGood <= xSeparation && xSeparation <= xEnvelope + xGood)
{
// get differences in max and target velocities
double vDifference = Math.Max(segMaxV - vTarget, 0);
// 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)) + vTarget;
// good distance
xDistanceToGood = xSeparation - xGood;
}
// otherwise xSeparation > xEnvelope
else
{
// can go max speed
vFollowing = segMaxV;
// good distance
xDistanceToGood = xSeparation - xGood;
}
// set out params
followingSpeed = vFollowing;
distanceToGood = xDistanceToGood;
}
/// <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 Maneuver PrimaryManeuver(ArbiterLane lane, ArbiterLane closestGood, VehicleState state, List<ITacticalBlockage> blockages)
{
// possible parameterizations
List<TravelingParameters> tps = new List<TravelingParameters>();
#region Nav
// get the next thing we need to stop at no matter what and parameters for stopping at it
ArbiterWaypoint navStop;
double navStopSpeed;
double navStopDistance;
StopType navStopType;
// make sure we stop an extra 3VL from any navigational stop
double extraDistance = 0.0;
// get next stop in this opposing lane
this.NextOpposingNavigationalStop(lane, closestGood, state.Front, extraDistance,
out navStopSpeed, out navStopDistance, out navStopType, out navStop);
// set global stop distance
this.NextNavigationStopDistance = navStopDistance;
// create parameterization of the stop
TravelingParameters navParams = this.NavStopParameterization(lane, navStopSpeed, navStopDistance, navStop, navStopType, state);
this.NaviationParameters = navParams;
// add to nav parames to arbiter information
CoreCommon.CurrentInformation.FQMBehavior = navParams.Behavior.ToShortString();
CoreCommon.CurrentInformation.FQMBehaviorInfo = navParams.Behavior.ShortBehaviorInformation();
CoreCommon.CurrentInformation.FQMSpeedCommand = navParams.Behavior.SpeedCommandString();
CoreCommon.CurrentInformation.FQMDistance = navParams.DistanceToGo.ToString("F6");
CoreCommon.CurrentInformation.FQMSpeed = navParams.RecommendedSpeed.ToString("F6");
CoreCommon.CurrentInformation.FQMState = navParams.NextState.ShortDescription();
CoreCommon.CurrentInformation.FQMStateInfo = navParams.NextState.StateInformation();
CoreCommon.CurrentInformation.FQMStopType = navStopType.ToString();
CoreCommon.CurrentInformation.FQMWaypoint = navStop.ToString();
CoreCommon.CurrentInformation.FQMSegmentSpeedLimit = lane.CurrentMaximumSpeed(state.Position).ToString("F1");
// add nav parameter
tps.Add(navParams);
#endregion
#region Vehicle
// forward vehicle update
this.ForwardVehicle.Update(lane, state);
if (this.ForwardVehicle.ShouldUseForwardTracker)
{
// get forward vehicle params
TravelingParameters vehicleParams = this.ForwardVehicle.Follow(lane, state);
// add vehicle param
tps.Add(vehicleParams);
}
#endregion
#region Blockages
/*
// get the blockage stop parameters
bool stopAtBlockage;
double blockageSpeed;
double blockageDistance;
this.StopForBlockages(lane, state, List<ITacticalBlockage> blockages, out stopAtBlockage, out blockageSpeed, out blockageDistance);
*/
#endregion
// sort params by most urgent
tps.Sort();
// set current
this.currentParamters = tps[0];
// out of navigation, blockages, and vehicle following determine the actual primary behavior parameters
return new Maneuver(tps[0].Behavior, tps[0].NextState, tps[0].Decorators, state.Timestamp);
}
/// <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);
}
}
