StanleyController.h

Go to the documentation of this file.

 
#ifndef STANLEY_CONTROLLER_H
#define STANLEY_CONTROLLER_H
 
#include "../../util/GeospatialOperations.hpp"
#include "../../util/NumberOperations.hpp"
 
// Put implicit includes in here.
 
#include <cmath>
#include <gtest/gtest.h>
#include <limits>
#include <vector>
 
 
 
namespace controllers
{
 
    class StanleyController
    {
        public:
            // Declare public methods and member variables.
            StanleyController(const double dKp = 1.0, const double dDistToFrontAxle = 1.0, const double dYawTolerance = 1.0);
            StanleyController(const std::vector<geoops::UTMCoordinate>& vUTMPath, const double dKp, const double dDistToFrontAxle, const double dYawTolerance);
            StanleyController(const std::vector<geoops::GPSCoordinate>& vGPSPath, const double dKp, const double dDistToFrontAxle, const double dYawTolerance);
            ~StanleyController();
 
            double Calculate(const geoops::UTMCoordinate& stUTMCurrPos, const double dVelocity, const double dCurrentHeading);
            double Calculate(const geoops::GPSCoordinate& stGPSCurrPos, const double dVelocity, const double dCurrentHeading);
            void ResetProgress();
 
            // Setters.
 
            void SetSteeringControlGain(const double dKpp);
            void SetDistanceToFrontAxle(const double dDistToFrontAxle);
            void SetYawTolerance(const double dYawTolerance);
 
            void SetPath(const std::vector<geoops::UTMCoordinate>& vUTMPath);
            void SetPath(const std::vector<geoops::GPSCoordinate>& vGPSPath);
            void SetPath(const std::vector<geoops::Waypoint>& vWaypointsPath);
 
            void ClearPath();
 
            // Getters.
 
            double GetSteeringControlGain() const;
            double GetDistanceToFrontAxle() const;
            double GetYawTolerance() const;
            unsigned int GetLastTargetIdx() const;
            std::vector<geoops::UTMCoordinate> GetPathUTM() const;
            std::vector<geoops::GPSCoordinate> GetPathGPS() const;
 
        private:
            // Declare private methods.
 
            geoops::UTMCoordinate CalculateFrontAxleCoordinate(const geoops::UTMCoordinate& stUTMCurrPos, const double dCurrentHeading) const;
            unsigned int IdentifyTargetIdx(const geoops::UTMCoordinate& stUTMFrontAxlePos) const;
            double CalculateTargetBearing(const unsigned int unTargetIdx) const;
            double CalculateCrossTrackError(const geoops::UTMCoordinate& stUTMFrontAxlePos, const unsigned int unTargetIdx, const double dCurrentHeading) const;
 
            // Declare private member variables.
 
            double m_dKp;                                     // Steering control gain.
            double m_dDistToFrontAxle;                        // Distance between the position sensor (GPS) and the front axle.
            double m_dYawTolerance;                           // Minimum yaw change threshold for execution.
            unsigned int m_unLastTargetIdx;                   // Index of last point on path used in Stanley calculation.
            std::vector<geoops::UTMCoordinate> m_vUTMPath;    // Stores the sequence of UTM coordinates defining the navigational path.
            std::vector<geoops::GPSCoordinate> m_vGPSPath;    // Stores the sequence of GPS coordinates defining the navigational path.
    };
}    // namespace controllers
 
#endif