/// <summary>
/// Next point at which to stop
/// </summary>
/// <param name="lane"></param>
/// <param name="position"></param>
/// <param name="stopPoint"></param>
/// <param name="stopSpeed"></param>
/// <param name="stopDistance"></param>
public void NextLaneStop(IFQMPlanable lane, Coordinates position, double[] enCovariance, List <ArbiterWaypoint> ignorable,
out ArbiterWaypoint stopPoint, out double stopSpeed, out double stopDistance, out StopType stopType)
{
// get the stop
List <WaypointType> wts = new List <WaypointType>();
wts.Add(WaypointType.Stop);
wts.Add(WaypointType.End);
stopPoint = lane.GetNext(position, wts, ignorable);
// set stop type
stopType = stopPoint.IsStop ? StopType.StopLine : StopType.EndOfLane;
// parameterize
this.StoppingParams(stopPoint, lane, position, enCovariance, out stopSpeed, out stopDistance);
}
/// <summary>
/// Next point at which to stop
/// </summary>
/// <param name="lane"></param>
/// <param name="position"></param>
/// <param name="stopPoint"></param>
/// <param name="stopSpeed"></param>
/// <param name="stopDistance"></param>
public void NextLaneStop(IFQMPlanable lane, Coordinates position, double[] enCovariance, List<ArbiterWaypoint> ignorable,
out ArbiterWaypoint stopPoint, out double stopSpeed, out double stopDistance, out StopType stopType)
{
// get the stop
List<WaypointType> wts = new List<WaypointType>();
wts.Add(WaypointType.Stop);
wts.Add(WaypointType.End);
stopPoint = lane.GetNext(position, wts, ignorable);
// set stop type
stopType = stopPoint.IsStop ? StopType.StopLine : StopType.EndOfLane;
// parameterize
this.StoppingParams(stopPoint, lane, position, enCovariance, out stopSpeed, out stopDistance);
}
/// <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>
/// 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);
}
}
