StuckDetection.hpp

Go to the documentation of this file.

 
#ifndef STUCK_DETECTION_HPP
#define STUCK_DETECTION_HPP
 
// Put implicit includes in here.
#include <chrono>
#include <cmath>
 
 
 
namespace statemachine
{
 
    class TimeIntervalBasedStuckDetector
    {
        private:
            // Declare private member variables.
 
            double m_dMaximumStuckCount;
            double m_dStuckCheckIntervalSeconds;
            double m_dVelocityThreshold;
            double m_dAngularVelocityThreshold;
            unsigned int m_unStuckChecksSoFar;
            std::chrono::system_clock::time_point m_tmTimeSinceLastStuckCheck;
 
        public:
            // Declare public class methods.
 
 
            TimeIntervalBasedStuckDetector(double dMaximumStuckCount         = 3,
                                           double dStuckCheckIntervalSeconds = 3,
                                           double dVelocityThreshold         = 0.3,
                                           double dAngularVelocityThreshold  = 5.0)
            {
                // Initialize member variables.
                m_dMaximumStuckCount         = dMaximumStuckCount;
                m_dStuckCheckIntervalSeconds = dStuckCheckIntervalSeconds;
                m_dVelocityThreshold         = dVelocityThreshold;
                m_dAngularVelocityThreshold  = dAngularVelocityThreshold;
                m_unStuckChecksSoFar         = 0;
                m_tmTimeSinceLastStuckCheck  = std::chrono::system_clock::now();
            }
 
 
            ~TimeIntervalBasedStuckDetector() {}
 
 
            bool CheckIfStuck(double dCurrentVelocity, double dCurrentAngularVelocity)
            {
                // Create instance variables.
                bool bStuck = false;
 
                // Time since we last checked if the rover is stuck.
                std::chrono::system_clock::time_point tmCurrentTime = std::chrono::system_clock::now();
                double dTimeSinceLastCheck = static_cast<double>(std::chrono::duration_cast<std::chrono::seconds>(tmCurrentTime - m_tmTimeSinceLastStuckCheck).count());
                if (dTimeSinceLastCheck > m_dStuckCheckIntervalSeconds)
                {
                    // Update time since last check to now.
                    m_tmTimeSinceLastStuckCheck = tmCurrentTime;
 
                    // Check if the rover is rotating or moving linearly.
                    if (std::abs(dCurrentVelocity) < m_dVelocityThreshold && std::abs(dCurrentAngularVelocity) < m_dAngularVelocityThreshold)
                    {
                        ++m_unStuckChecksSoFar;
                    }
                    else
                    {
                        m_unStuckChecksSoFar = 0;
                    }
                }
 
                // Check if we met stuck criteria.
                if (m_unStuckChecksSoFar > m_dMaximumStuckCount)
                {
                    // Reset stuck checks.
                    m_unStuckChecksSoFar = 0;
                    // Set stuck toggle.
                    bStuck = true;
                }
 
                // Return if stuck or not.
                return bStuck;
            }
    };
}    // namespace statemachine
 
#endif