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