PredictiveStanleyController.h

Go to the documentation of this file.

 
#ifndef PREDICTIVE_STANLEY_CONTROLLER_H
#define PREDICTIVE_STANLEY_CONTROLLER_H
 
#include "../../util/GeospatialOperations.hpp"
#include "../../util/logging/PathTracer2D.hpp"
#include "../kinematics/BicycleModel.hpp"
 
#include <vector>
 
 
 
namespace controllers
{
 
    class PredictiveStanleyController
    {
        public:
            // Declare public class structs.
            struct DriveVector
            {
                public:
                    double dThetaHeading;
                    double dVelocity;
            };
 
            // Declare public class methods.
            PredictiveStanleyController();
            PredictiveStanleyController(const double dControlGain,
                                        const double dSteeringAngleLimit,
                                        const double dWheelbase,
                                        const int nPredictionHorizon,
                                        const double dPredictionTimeStep);
            ~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 SetSteeringAngleLimit(const double dSteeringAngleLimit);
            void SetWheelbase(const double dWheelbase);
 
            // Getters.
 
            std::vector<geoops::Waypoint> GetReferencePath() const;
            double GetControlGain() const;
            double GetSteeringAngleLimit() const;
            double GetWheelbase() const;
            double GetReferencePathTargetIndex() const;
 
        private:
            // Declare private class methods.
 
            geoops::Waypoint FindClosestWaypointInPath(const geoops::UTMCoordinate& stCurrentPosition, const double dCurrentHeading);
 
            // Declare private member variables.
 
            BicycleModel m_BicycleModel;
            double m_dControlGain;
            double m_dSteeringAngleLimit;
            double m_dWheelbase;
            int m_nPredictionHorizon;
            double m_dPredictionTimeStep;
            int m_nCurrentReferencePathTargetIndex;
            std::vector<double> m_vReferencePathCurvature;
            std::vector<geoops::Waypoint> m_vReferencePath;
    };
}    // namespace controllers
#endif