/// <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>
/// Updates the current vehicle
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
public void Update(IFQMPlanable lane, VehicleState state)
{
// get the forward path
LinePath p = lane.LanePath();
// get our position
Coordinates f = state.Front;
// get all vehicles associated with those components
List <VehicleAgent> vas = new List <VehicleAgent>();
foreach (IVehicleArea iva in lane.AreaComponents)
{
if (TacticalDirector.VehicleAreas.ContainsKey(iva))
{
vas.AddRange(TacticalDirector.VehicleAreas[iva]);
}
}
// get the closest forward of us
double minDistance = Double.MaxValue;
VehicleAgent closest = null;
// set the vehicles to ignore
this.VehiclesToIgnore = new List <int>();
// get clsoest
foreach (VehicleAgent va in vas)
{
// get position of front
Coordinates frontPos = va.ClosestPosition;
// check relatively inside
if (lane.RelativelyInside(frontPos))
{
// distance to vehicle
double frontDist = lane.DistanceBetween(f, frontPos);
// check forward of us
if (frontDist > 0)
{
// add to ignorable
this.VehiclesToIgnore.Add(va.VehicleId);
// check for closest
if (frontDist < minDistance)
{
minDistance = frontDist;
closest = va;
}
}
}
}
this.CurrentVehicle = closest;
this.currentDistance = minDistance;
}
/// <summary>
/// Distance from coord to vehicle along lane
/// </summary>
/// <param name="lane"></param>
/// <param name="c"></param>
/// <returns></returns>
public double DistanceToVehicle(IFQMPlanable lane, Coordinates c)
{
if (CurrentVehicle != null)
{
return(lane.DistanceBetween(c, CurrentVehicle.ClosestPosition));
}
else
{
return(Double.MaxValue);
}
}
/// <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 double RequiredSpeed(ArbiterWaypoint waypoint, double speedAtWaypoint, double currentMax, IFQMPlanable lane, Coordinates position)
{
// get dist to waypoint
double stopSpeedDistance = lane.DistanceBetween(position, waypoint.Position);
// segment max speed
double segmentMaxSpeed = currentMax;
// distance to stop from max v given desired acceleration
double stopEnvelopeLength = (Math.Pow(speedAtWaypoint, 2) - Math.Pow(currentMax, 2)) / (2.0 * -0.5);
// check if we are within profile
if (stopSpeedDistance > 0 && stopSpeedDistance < stopEnvelopeLength)
{
// get speed along profile
double requiredSpeed = speedAtWaypoint + ((stopSpeedDistance / stopEnvelopeLength) * (segmentMaxSpeed - speedAtWaypoint));
return(requiredSpeed);
}
else
{
return(segmentMaxSpeed);
}
}
/// <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>
/// 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>
/// 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 double RequiredSpeed(ArbiterWaypoint waypoint, double speedAtWaypoint, double currentMax, IFQMPlanable lane, Coordinates position)
{
// get dist to waypoint
double stopSpeedDistance = lane.DistanceBetween(position, waypoint.Position);
// segment max speed
double segmentMaxSpeed = currentMax;
// distance to stop from max v given desired acceleration
double stopEnvelopeLength = (Math.Pow(speedAtWaypoint, 2) - Math.Pow(currentMax, 2)) / (2.0 * -0.5);
// check if we are within profile
if (stopSpeedDistance > 0 && stopSpeedDistance < stopEnvelopeLength)
{
// get speed along profile
double requiredSpeed = speedAtWaypoint + ((stopSpeedDistance / stopEnvelopeLength) * (segmentMaxSpeed - speedAtWaypoint));
return requiredSpeed;
}
else
{
return segmentMaxSpeed;
}
}
/// <summary>
/// Updates the current vehicle
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
public void Update(IFQMPlanable lane, VehicleState state)
{
// get the forward path
LinePath p = lane.LanePath();
// get our position
Coordinates f = state.Front;
// get all vehicles associated with those components
List<VehicleAgent> vas = new List<VehicleAgent>();
foreach (IVehicleArea iva in lane.AreaComponents)
{
if (TacticalDirector.VehicleAreas.ContainsKey(iva))
vas.AddRange(TacticalDirector.VehicleAreas[iva]);
}
// get the closest forward of us
double minDistance = Double.MaxValue;
VehicleAgent closest = null;
// set the vehicles to ignore
this.VehiclesToIgnore = new List<int>();
// get clsoest
foreach (VehicleAgent va in vas)
{
// get position of front
Coordinates frontPos = va.ClosestPosition;
// check relatively inside
if (lane.RelativelyInside(frontPos))
{
// distance to vehicle
double frontDist = lane.DistanceBetween(f, frontPos);
// check forward of us
if (frontDist > 0)
{
// add to ignorable
this.VehiclesToIgnore.Add(va.VehicleId);
// check for closest
if (frontDist < minDistance)
{
minDistance = frontDist;
closest = va;
}
}
}
}
this.CurrentVehicle = closest;
this.currentDistance = minDistance;
}
/// <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>
/// Distance from coord to vehicle along lane
/// </summary>
/// <param name="lane"></param>
/// <param name="c"></param>
/// <returns></returns>
public double DistanceToVehicle(IFQMPlanable lane, Coordinates c)
{
if (CurrentVehicle != null)
return lane.DistanceBetween(c, CurrentVehicle.ClosestPosition);
else
return Double.MaxValue;
}
