/// <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);
        }
Exemplo n.º 3
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);
            }
        }