PurePursuitController.h

Go to the documentation of this file.

 
#ifndef PURE_PURSUIT_CONTROLLER_H
#define PURE_PURSUIT_CONTROLLER_H
 
#include "../../util/GeospatialOperations.hpp"
 
#include <vector>
 
 
 
namespace controllers
{
 
    class PurePursuitController
    {
        public:
            // Declare public structs.
 
 
            struct DriveVector
            {
                public:
                    double dThetaHeading;
                    double dVelocity;
            };
 
            // Constructors and Destructors.
 
            PurePursuitController(const double dLookaheadDistance = 2.0, const int nLookaheadIndex = 5);
            ~PurePursuitController() = default;
 
            // Declare public class methods.
 
            DriveVector Calculate(const geoops::RoverPose& stCurrentPose, const double dMaxSpeed);
 
            // Setters.
 
            void SetReferencePath(const std::vector<geoops::Waypoint>& vReferencePath);
            void SetLookaheadDistance(const double dLookaheadDistance);
            void SetLookaheadIndex(const int nLookaheadIndex);
 
            // Getters.
 
            std::vector<geoops::Waypoint> GetReferencePath() const;
            double GetLookaheadDistance() const;
            int GetReferencePathTargetIndex() const;
 
        private:
            // Declare private class methods.
 
            int FindClosestWaypointIndex(const geoops::UTMCoordinate& stCurrentPosition);
            geoops::Waypoint FindLookaheadWaypoint(const geoops::UTMCoordinate& stCurrentPosition);
 
            // Declare private member variables.
 
            double m_dLookaheadDistance;
            int m_nLookaheadIndex;
            int m_nCurrentReferencePathTargetIndex;
            std::vector<geoops::Waypoint> m_vReferencePath;
    };
}    // namespace controllers
#endif