BicycleModel.hpp

Go to the documentation of this file.

 
#ifndef BICYCLE_MODEL_H
#define BICYCLE_MODEL_H
 
#include "../../util/NumberOperations.hpp"
 
#include <chrono>
#include <cmath>
#include <vector>
 
 
 
class BicycleModel
{
    public:
        // Declare public structs for this class.
 
 
        struct Prediction
        {
            public:
                double dXPosition;
                double dYPosition;
                double dTheta;
        };
 
        // Declare public class methods.
 
 
        BicycleModel()
        {
            // Initialize member variables.
            m_dWheelbase       = 0.0;
            m_dXPosition       = 0.0;
            m_dYPosition       = 0.0;
            m_dTheta           = 0.0;
            m_dVelocity        = -1.0;
            m_dSteeringAngle   = 0.0;
            m_tmLastUpdateTime = std::chrono::system_clock::now();
        }
 
 
        BicycleModel(const double dWheelbase, const double dXPosition, const double dYPosition, const double dTheta)
        {
            // Initialize member variables.
            m_dWheelbase       = dWheelbase;
            m_dXPosition       = dXPosition;
            m_dYPosition       = dYPosition;
            m_dTheta           = dTheta;
            m_dVelocity        = -1.0;
            m_dSteeringAngle   = 0.0;
            m_tmLastUpdateTime = std::chrono::system_clock::now();
        }
 
 
        void ResetState(const double dXPosition, const double dYPosition, const double dTheta)
        {
            // Update member variables.
            m_dXPosition       = dXPosition;
            m_dYPosition       = dYPosition;
            m_dTheta           = dTheta;
            m_dSteeringAngle   = 0.0;
            m_dVelocity        = -1.0;
            m_tmLastUpdateTime = std::chrono::system_clock::now();
        }
 
 
        void ResetState()
        {
            // Update member variables.
            m_dXPosition       = 0.0;
            m_dYPosition       = 0.0;
            m_dTheta           = 0.0;
            m_dSteeringAngle   = 0.0;
            m_dVelocity        = -1.0;
            m_tmLastUpdateTime = std::chrono::system_clock::now();
        }
 
 
        void UpdateState(const double dXPosition, const double dYPosition, const double dTheta)
        {
            // Check if this is our first update.
            if (m_dVelocity == -1.0)
            {
                // Set the velocity to zero.
                m_dVelocity = 0.0;
            }
            // Calculate the velocity of the rover as long as the new position is different from the current position.
            else if (dXPosition != m_dXPosition || dYPosition != m_dYPosition)
            {
                // Calculate the velocity of the rover.
                std::chrono::system_clock::time_point tmCurrentTime = std::chrono::system_clock::now();
                double dTimeElapsed = std::chrono::duration_cast<std::chrono::milliseconds>(tmCurrentTime - m_tmLastUpdateTime).count() / 1000.0;
                m_dVelocity         = std::sqrt(std::pow(dXPosition - m_dXPosition, 2) + std::pow(dYPosition - m_dYPosition, 2)) / dTimeElapsed;
 
                // Update the last update time.
                m_tmLastUpdateTime = tmCurrentTime;
            }
 
            // Calculate the steering angle of the rover.
            m_dSteeringAngle = (std::atan2(dYPosition - m_dYPosition, dXPosition - m_dXPosition) * 180.0 / M_PI) - m_dTheta;
            // Ensure the steering angle stays within 0-360 degrees.
            m_dSteeringAngle = numops::InputAngleModulus(m_dSteeringAngle, 0.0, 360.0);
 
            // Update member variables.
            m_dXPosition = dXPosition;
            m_dYPosition = dYPosition;
            m_dTheta     = dTheta;
        }
 
 
        void Predict(const double dTimeStep, const int nNumPredictions, std::vector<Prediction>& vPredictions)
        {
            // Start from the current state.
            double dXPredicted     = m_dXPosition;
            double dYPredicted     = m_dYPosition;
            double dThetaPredicted = m_dTheta;
 
            // Perform prediction for a specified number of time steps.
            for (int nIter = 0; nIter < nNumPredictions; ++nIter)
            {
                // Convert theta from degrees to radians for calculation.
                double dThetaRad         = dThetaPredicted * M_PI / 180.0;
                double dSteeringAngleRad = m_dSteeringAngle * M_PI / 180.0;
 
                // Calculate the new state.
                dXPredicted += m_dVelocity * std::sin(dThetaRad) * dTimeStep;
                dYPredicted += m_dVelocity * std::cos(dThetaRad) * dTimeStep;
                dThetaPredicted += (m_dVelocity / m_dWheelbase) * std::tan(dSteeringAngleRad - M_PI_2) * dTimeStep;
 
                // Ensure theta stays within 0-360 degrees.
                dThetaPredicted = numops::InputAngleModulus(dThetaPredicted, 0.0, 360.0);
 
                // Store the new state.
                Prediction stPrediction{dXPredicted, dYPredicted, dThetaPredicted};
                vPredictions.push_back(stPrediction);
            }
        }
 
        // Setters.
 
 
        void SetWheelbase(const double dWheelbase) { m_dWheelbase = dWheelbase; }
 
 
        void SetXPosition(const double dXPosition) { m_dXPosition = dXPosition; }
 
 
        void SetYPosition(const double dYPosition) { m_dYPosition = dYPosition; }
 
 
        void SetTheta(const double dTheta) { m_dTheta = dTheta; }
 
 
        void SetSteeringAngle(const double dSteeringAngle) { m_dSteeringAngle = dSteeringAngle; }
 
        // Getters.
 
 
        double GetWheelbase() const { return m_dWheelbase; }
 
 
        double GetXPosition() const { return m_dXPosition; }
 
 
        double GetYPosition() const { return m_dYPosition; }
 
 
        double GetTheta() const { return m_dTheta; }
 
 
        double GetVelocity() const { return m_dVelocity; }
 
 
        double GetSteeringAngle() const { return m_dSteeringAngle; }
 
    private:
        // Declare private member variables.
 
        double m_dWheelbase;
        double m_dXPosition;
        double m_dYPosition;
        double m_dTheta;
        double m_dVelocity;
        double m_dSteeringAngle;
        std::chrono::system_clock::time_point m_tmLastUpdateTime;
};
#endif