PredictiveStanleyController.h

Go to the documentation of this file.

 
#ifndef PREDICTIVE_STANLEY_CONTROLLER_H
#define PREDICTIVE_STANLEY_CONTROLLER_H
 
#include "../../util/GeospatialOperations.hpp"
#include "../kinematics/UnicycleModel.hpp"
 
#include <utility>
#include <vector>
 
 
 
namespace controllers
{
 
    class PredictiveStanleyController
    {
        public:
            // Declare public class structs.
            struct DriveVector
            {
                public:
                    double dThetaHeading;
                    double dVelocity;
            };
 
            // Declare public class methods.
            PredictiveStanleyController(const double dControlGain          = 0.1,
                                        const double dAngularVelocityLimit = 90.0,
                                        const int nPredictionHorizon       = 5,
                                        const double dPredictionTimeStep   = 0.01);
            ~PredictiveStanleyController();
            DriveVector Calculate(const geoops::RoverPose& stCurrentPose, const double dMaxSpeed = constants::NAVIGATING_MOTOR_POWER);
 
            // Setters.
 
            void SetReferencePath(const std::vector<geoops::Waypoint>& vReferencePath);
            void SetReferencePath(const std::vector<geoops::UTMCoordinate>& vReferencePath);
            void SetReferencePath(const std::vector<geoops::GPSCoordinate>& vReferencePath);
            void SetControlGain(const double dControlGain);
            void SetAngularVelocityLimit(const double dAngularVelocityLimit);
 
            // Getters.
 
            std::vector<geoops::Waypoint> GetReferencePath() const;
            double GetControlGain() const;
            double GetAngularVelocityLimit() const;
            double GetReferencePathTargetIndex() const;
 
        private:
            // Declare private class methods.
 
            std::pair<geoops::Waypoint, int> FindClosestWaypointInPath(const geoops::UTMCoordinate& stCurrentPosition, const int nStartIndex) const;
 
            // Declare private member variables.
 
            UnicycleModel m_UnicycleModel;
            double m_dControlGain;
            double m_dAngularVelocityLimit;
            int m_nPredictionHorizon;
            double m_dPredictionTimeStep;
            int m_nCurrentReferencePathTargetIndex;
            std::vector<geoops::Waypoint> m_vReferencePath;
    };
}    // namespace controllers
#endif