/// <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(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>
/// 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>
/// 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>
/// 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);
}
}