This class should be instantiated within another state to be used for detection of if the rover is stuck. Stuck detection is solely based off of a check interval on the current velocity and rotation. If the velocity and rotation are non-moving for more then a maximum interval count, we are considered stuck. More...

#include <StuckDetection.hpp>

Public Member Functions
	TimeIntervalBasedStuckDetector (double dMaximumStuckCount=3, double dStuckCheckIntervalSeconds=3)
	Construct a new Stuck Detector object.

	~TimeIntervalBasedStuckDetector ()
	Destroy the Stuck Detector object.

bool	CheckIfStuck (double dCurrentVelocity, double dCurrentAngularVelocity, double dVelocityThreshold=0.1, double dAngularVelocityThreshold=0.1)
	Checks if the rover meets stuck criteria based in the given parameters.

void	ResetStuckChecks ()
	Reset stuck variables so rover does not go into stuck-state next check of CheckIfStuck.

Private Attributes
double	m_dMaximumStuckCount

double	m_dStuckCheckIntervalSeconds

unsigned int	m_unStuckChecksSoFar

std::chrono::system_clock::time_point	m_tmTimeSinceLastStuckCheck

Detailed Description

This class should be instantiated within another state to be used for detection of if the rover is stuck. Stuck detection is solely based off of a check interval on the current velocity and rotation. If the velocity and rotation are non-moving for more then a maximum interval count, we are considered stuck.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-04-23

Constructor & Destructor Documentation

◆ TimeIntervalBasedStuckDetector()

statemachine::TimeIntervalBasedStuckDetector::TimeIntervalBasedStuckDetector	(	double	dMaximumStuckCount = `3`,
		double	dStuckCheckIntervalSeconds = `3`
	)

inline

Construct a new Stuck Detector object.

Parameters

dMaximumStuckCount	- The maximum number of times the rover can be not moving upon check interval before it is considered stuck.
dStuckCheckIntervalSeconds	- The interval in seconds that the function should check the current rover velocity and angular movement.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-04-23

            {
                // Initialize member variables.
                m_dMaximumStuckCount         = dMaximumStuckCount;
                m_dStuckCheckIntervalSeconds = dStuckCheckIntervalSeconds;
                m_unStuckChecksSoFar         = 0;
                m_tmTimeSinceLastStuckCheck  = std::chrono::system_clock::now();
            }

◆ ~TimeIntervalBasedStuckDetector()

statemachine::TimeIntervalBasedStuckDetector::~TimeIntervalBasedStuckDetector ( )

inline

Destroy the Stuck Detector object.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-04-23

84{}

Member Function Documentation

◆ CheckIfStuck()

bool statemachine::TimeIntervalBasedStuckDetector::CheckIfStuck	(	double	dCurrentVelocity,
		double	dCurrentAngularVelocity,
		double	dVelocityThreshold = `0.1`,
		double	dAngularVelocityThreshold = `0.1`
	)

inline

Checks if the rover meets stuck criteria based in the given parameters.

Returns: true - The rover is stuck.; false - The is not stuck.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om), Jason Pittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-04-23

            {
                // 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 = std::chrono::duration_cast<std::chrono::milliseconds>(tmCurrentTime - m_tmTimeSinceLastStuckCheck).count() / 1000.0;
                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) < dVelocityThreshold && std::abs(dCurrentAngularVelocity) < 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;
            }