statemachine Namespace Reference

Namespace containing all state machine related classes. More...

Classes
class	ApproachingMarkerState
	The ApproachingMarkerState class implements the Approaching Marker state for the Autonomy State Machine. More...
class	ApproachingObjectState
	The ApproachingObjectState class implements the Approaching Object state for the Autonomy State Machine. More...
class	IdleState
	The IdleState class implements the Idle state for the Autonomy State Machine. More...
class	NavigatingState
	The NavigatingState class implements the Navigating state for the Autonomy State Machine. More...
class	ReversingState
	The ReversingState class implements the Reversing state for the Autonomy State Machine. More...
class	SearchPatternState
	The SearchPatternState class implements the Search Pattern state for the Autonomy State Machine. More...
class	State
	The abstract state class. All states inherit from this class. More...
class	StuckState
	The StuckState class implements the Stuck state for the Autonomy State Machine. More...
class	TimeIntervalBasedStuckDetector
	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...
class	VerifyingMarkerState
	The VerifyingMarkerState class implements the Verifying Marker state for the Autonomy State Machine. More...
class	VerifyingObjectState
	The VerifyingObjectState class implements the Verifying Object state for the Autonomy State Machine. More...
class	VerifyingPositionState
	The VerifyingPositionState class implements the Verifying Position state for the Autonomy State Machine. More...

Enumerations
enum class	States {   eIdle , eNavigating , eSearchPattern , eApproachingMarker ,   eApproachingObject , eVerifyingPosition , eVerifyingMarker , eVerifyingObject ,   eReversing , eStuck , NUM_STATES }
	The states that the state machine can be in. More...
enum class	Event {   eStart , eReachedGpsCoordinate , eReachedMarker , eReachedObject ,   eMarkerSeen , eObjectSeen , eMarkerUnseen , eObjectUnseen ,   eVerifyingComplete , eVerifyingFailed , eAbort , eRestart ,   eNoWaypoint , eNewWaypoint , eReverse , eReverseComplete ,   eSearchFailed , eStuck , eUnstuck , NUM_EVENTS }
	The events that can be triggered in the state machine. More...

Functions
std::string	StateToString (States eState)
	Converts a state object to a string.
void	LoadDetectedObjects (std::vector< objectdetectutils::Object > &vDetectedObjects, const std::vector< std::shared_ptr< ObjectDetector > > &vObjectDetectors)
	Aggregates all detected objects from each provided object detector.
int	IdentifyTargetObject (const std::vector< std::shared_ptr< ObjectDetector > > &vObjectDetectors, objectdetectutils::Object &stObjectTarget, const geoops::WaypointType &eDesiredDetectionType=geoops::WaypointType::eUNKNOWN)
	Identify a target object in the rover's vision, using Torch detection.
void	LoadDetectedTags (std::vector< tagdetectutils::ArucoTag > &vDetectedArucoTags, const std::vector< std::shared_ptr< TagDetector > > &vTagDetectors)
	Aggregates all detected tags from each provided tag detector for both OpenCV and YOLO detection.
int	IdentifyTargetMarker (const std::vector< std::shared_ptr< TagDetector > > &vTagDetectors, tagdetectutils::ArucoTag &stArucoTarget, tagdetectutils::ArucoTag &stTorchTarget, const int nTargetTagID=static_cast< int >(manifest::Autonomy::AUTONOMYWAYPOINTTYPES::ANY))
	Identify a target marker in the rover's vision, using OpenCV detection.

Detailed Description

Namespace containing all state machine related classes.

Author

Eli Byrd (edbgk.nosp@m.k@ms.nosp@m.t.edu)

Date

2024-01-17

Author

Sam Hajdukiewicz (saman.nosp@m.thah.nosp@m.ajduk.nosp@m.iewi.nosp@m.cz@gm.nosp@m.ail..nosp@m.com)

Date

2024-01-17

Author

Eli Byrd (edbgk.nosp@m.k@ms.nosp@m.t.edu)

Date

2024-05-24

Author

Sam Hajdukiewicz (saman.nosp@m.thah.nosp@m.ajduk.nosp@m.iewi.nosp@m.cz@gm.nosp@m.ail..nosp@m.com)

Date

2025-05-08

Author

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

Date

2024-04-23

Author

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

Date

2025-04-04

Enumeration Type Documentation

States

enum class statemachine::States

strong

The states that the state machine can be in.

Author

Eli Byrd (edbgk.nosp@m.k@ms.nosp@m.t.edu)

Date

2024-01-18

    {
        eIdle,
        eNavigating,
        eSearchPattern,
        eApproachingMarker,
        eApproachingObject,
        eVerifyingPosition,
        eVerifyingMarker,
        eVerifyingObject,
        eReversing,
        eStuck,
        NUM_STATES
    };

Event

enum class statemachine::Event

strong

The events that can be triggered in the state machine.

Author

Eli Byrd (edbgk.nosp@m.k@ms.nosp@m.t.edu)

Date

2024-01-18

    {
        eStart,
        eReachedGpsCoordinate,
        eReachedMarker,
        eReachedObject,
        eMarkerSeen,
        eObjectSeen,
        eMarkerUnseen,
        eObjectUnseen,
        eVerifyingComplete,
        eVerifyingFailed,
        eAbort,
        eRestart,
        eNoWaypoint,
        eNewWaypoint,
        eReverse,
        eReverseComplete,
        eSearchFailed,
        eStuck,
        eUnstuck,
 
        NUM_EVENTS
    };

Function Documentation

StateToString()

std::string statemachine::StateToString ( States  eState )

inline

Converts a state object to a string.

Parameters

eState

-

Returns

std::string -

Author

Eli Byrd (edbgk.nosp@m.k@ms.nosp@m.t.edu)

Date

2024-01-18

    {
        switch (eState)
        {
            case States::eIdle: return "Idle";
            case States::eNavigating: return "Navigating";
            case States::eSearchPattern: return "Search Pattern";
            case States::eApproachingMarker: return "Approaching Marker";
            case States::eApproachingObject: return "Approaching Object";
            case States::eVerifyingPosition: return "Verifying Position";
            case States::eVerifyingMarker: return "Verifying Marker";
            case States::eVerifyingObject: return "Verifying Object";
            case States::eReversing: return "Reversing";
            case States::eStuck: return "Stuck";
            default: return "Unknown";
        }
    }

Here is the caller graph for this function:

LoadDetectedObjects()

void statemachine::LoadDetectedObjects ( std::vector< objectdetectutils::Object > &  vDetectedObjects, const std::vector< std::shared_ptr< ObjectDetector > > &  vObjectDetectors  )

inline

Aggregates all detected objects from each provided object detector.

Parameters

vDetectedObjects	- Reference vector that will hold all of the aggregated detected objects.
vObjectDetectors	- Vector of pointers to object detectors that will be used to request their detected objects.

Author

Sam Hajdukiewicz (saman.nosp@m.thah.nosp@m.ajduk.nosp@m.iewi.nosp@m.cz@gm.nosp@m.ail..nosp@m.com)

Date

2025-05-08

    {
        // Number of object detectors.
        size_t siNumObjectDetectors = vObjectDetectors.size();
 
        // Initialize vectors to store detected objects temporarily.
        std::vector<std::vector<objectdetectutils::Object>> vDetectedObjectBuffers(siNumObjectDetectors);
 
        // Initialize vectors to store detected objects futures.
        std::vector<std::future<bool>> vDetectedObjectsFuture;
 
        // Track exactly which cameras successfully spawned a future to prevent vector crashes.
        std::vector<bool> vSpawnedFuture(siNumObjectDetectors, false);
 
        // Request objects from each detector.
        for (size_t siIdx = 0; siIdx < siNumObjectDetectors; ++siIdx)
        {
            // Check if this object detector is ready.
            if (vObjectDetectors[siIdx]->GetIsReady())
            {
                // Request detected objects from detector.
                vDetectedObjectsFuture.emplace_back(vObjectDetectors[siIdx]->RequestDetectedObjects(vDetectedObjectBuffers[siIdx]));
                vSpawnedFuture[siIdx] = true;
            }
        }
 
        // Ensure all requests have been fulfilled.
        // Then transfer objects from the buffer to vDetectedObjects for the user to access.
        int nFutureIdx = 0;
        // Iterate through the total number of detectors to match the buffer and spawned tracking sizes.
        for (size_t siIdx = 0; siIdx < siNumObjectDetectors; ++siIdx)
        {
            // Only check the buffer if the detector was ready and actually spawned a future
            if (vSpawnedFuture[siIdx])
            {
                // Wait for the correct future to finish
                vDetectedObjectsFuture[nFutureIdx].get();
                nFutureIdx++;
 
                // Loop through the detected objects and add them to the vDetectedObjects vector.
                for (const objectdetectutils::Object& tObject : vDetectedObjectBuffers[siIdx])
                {
                    vDetectedObjects.emplace_back(tObject);
                }
            }
        }
    }

Here is the caller graph for this function:

IdentifyTargetObject()

int statemachine::IdentifyTargetObject ( const std::vector< std::shared_ptr< ObjectDetector > > &  vObjectDetectors, objectdetectutils::Object &  stObjectTarget, const geoops::WaypointType &  eDesiredDetectionType = geoops::WaypointType::eUNKNOWN  )

inline

Identify a target object in the rover's vision, using Torch detection.

Note

If multiple objects are detected the closest one will be chosen as the target.

Parameters

vObjectDetectors	- The vector of object detectors to use for detection.
stObjectTarget	- The detected object marker from Torch.
eDesiredDetectionType	- The desired detection type to check for.

Returns

int - The total number of objects currently detected.

Author

Sam Hajdukiewicz (saman.nosp@m.thah.nosp@m.ajduk.nosp@m.iewi.nosp@m.cz@gm.nosp@m.ail..nosp@m.com)

Date

2025-05-09

    {
        // Create instance variables.
        std::vector<objectdetectutils::Object> vDetectedObjects;
        objectdetectutils::Object stBestObject;
        std::string szIdentifiedObjects = "";
 
        // Initialize best percentage to 0 so the first valid object always wins
        double dBestAreaPercentage = 0.0;
 
        // Get the current time
        std::chrono::system_clock::time_point tmCurrentTime = std::chrono::system_clock::now();
 
        // Load all detected objects in the rover's vision.
        LoadDetectedObjects(vDetectedObjects, vObjectDetectors);
 
        // Find the best object.
        for (const objectdetectutils::Object& stCandidate : vDetectedObjects)
        {
            // Calculate the total age of the object.
            double dObjectTotalAge = std::fabs(std::chrono::duration_cast<std::chrono::milliseconds>(tmCurrentTime - stCandidate.tmCreation).count() / 1000.0);
 
            // Null pointer safety check for the bounding box
            if (stCandidate.pBoundingBox == nullptr)
            {
                continue;
            }
 
            // Calculate the total object area and percentage of the screen the object takes up.
            double dArea           = stCandidate.pBoundingBox->area();
            double dAreaPercentage = (dArea / (stCandidate.cvImageResolution.width * stCandidate.cvImageResolution.height)) * 100.0;
 
            // Determine the desired detection type.
            switch (eDesiredDetectionType)
            {
                case geoops::WaypointType::eMalletWaypoint:
                {
                    if (stCandidate.eDetectionType != objectdetectutils::ObjectDetectionType::eMallet)
                        continue;
                    break;
                }
                case geoops::WaypointType::eWaterBottleWaypoint:
                {
                    if (stCandidate.eDetectionType != objectdetectutils::ObjectDetectionType::eWaterBottle)
                        continue;
                    break;
                }
                case geoops::WaypointType::eRockPickWaypoint:
                {
                    if (stCandidate.eDetectionType != objectdetectutils::ObjectDetectionType::eRockPick)
                        continue;
                    break;
                }
                case geoops::WaypointType::eObjectWaypoint:
                {
                    if (stCandidate.eDetectionType != objectdetectutils::ObjectDetectionType::eMallet &&
                        stCandidate.eDetectionType != objectdetectutils::ObjectDetectionType::eWaterBottle &&
                        stCandidate.eDetectionType != objectdetectutils::ObjectDetectionType::eRockPick)
                    {
                        continue;
                    }
                    break;
                }
                default:
                {
                    break;
                }
            }
 
            // Check the object detection method type.
            if (stCandidate.eDetectionMethod == objectdetectutils::ObjectDetectionMethod::eTorch)
            {
                // Assemble the identified objects string.
                szIdentifiedObjects += "\tObject Class: " + stCandidate.szClassName + " Object Age: " + std::to_string(dObjectTotalAge) +
                                       "s Object Screen Percentage: " + std::to_string(dAreaPercentage) + "%\n";
 
                // Check if the object meets the threshold requirements.
                if (dAreaPercentage < constants::BBOX_MIN_SCREEN_PERCENTAGE || dObjectTotalAge < constants::BBOX_MIN_LIFETIME_THRESHOLD)
                {
                    continue;
                }
 
                // Prioritize the object that takes up the most screen area
                if (dAreaPercentage > dBestAreaPercentage)
                {
                    // Set the target object to the detected object.
                    stBestObject        = stCandidate;
                    dBestAreaPercentage = dAreaPercentage;
                }
            }
        }
 
        // Only print the identified objects if there are any.
        if (stBestObject.dConfidence != 0.0)
        {
            // Submit logger message.
            LOG_DEBUG(logging::g_qSharedLogger, "ObjectDetectionChecker: Identified objects:\n{}", szIdentifiedObjects);
        }
 
        // Set the target object to the best object.
        stObjectTarget = stBestObject;
 
        return static_cast<int>(vDetectedObjects.size());
    }

Here is the call graph for this function:

Here is the caller graph for this function:

LoadDetectedTags()

void statemachine::LoadDetectedTags ( std::vector< tagdetectutils::ArucoTag > &  vDetectedArucoTags, const std::vector< std::shared_ptr< TagDetector > > &  vTagDetectors  )

inline

Aggregates all detected tags from each provided tag detector for both OpenCV and YOLO detection.

Parameters

vDetectedArucoTags	- Reference vector that will hold all of the aggregated detected Aruco tags.
vTagDetectors	- Vector of pointers to tag detectors that will be used to request their detected tags.

Author

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

Date

2025-04-04

    {
        // Number of tag detectors.
        size_t siNumTagDetectors = vTagDetectors.size();
 
        // Initialize vectors to store detected tags temporarily.
        std::vector<std::vector<tagdetectutils::ArucoTag>> vDetectedArucoTagBuffers(siNumTagDetectors);
 
        // Initialize vectors to store detected tags futures.
        std::vector<std::future<bool>> vDetectedArucoTagsFuture;
 
        // Track exactly which cameras successfully spawned a future to prevent vector crashes.
        std::vector<bool> vSpawnedFuture(siNumTagDetectors, false);
 
        // Request tags from each detector.
        for (size_t siIdx = 0; siIdx < siNumTagDetectors; ++siIdx)
        {
            // Check if this tag detector is ready.
            if (vTagDetectors[siIdx]->GetIsReady())
            {
                // Request detected Aruco tags from detector.
                vDetectedArucoTagsFuture.emplace_back(vTagDetectors[siIdx]->RequestDetectedArucoTags(vDetectedArucoTagBuffers[siIdx]));
                vSpawnedFuture[siIdx] = true;
            }
        }
 
        // Ensure all requests have been fulfilled.
        int nFutureIdx = 0;
        for (size_t siIdx = 0; siIdx < siNumTagDetectors; ++siIdx)
        {
            // Only check the buffer if the detector was ready and actually spawned a future
            if (vSpawnedFuture[siIdx])
            {
                // Wait for the correct future to finish
                vDetectedArucoTagsFuture[nFutureIdx].get();
                nFutureIdx++;
 
                // Loop through the detected tags using the correct buffer index (siIdx)
                for (const tagdetectutils::ArucoTag& tTag : vDetectedArucoTagBuffers[siIdx])
                {
                    vDetectedArucoTags.emplace_back(tTag);
                }
            }
        }
    }

Here is the caller graph for this function:

IdentifyTargetMarker()

int statemachine::IdentifyTargetMarker ( const std::vector< std::shared_ptr< TagDetector > > &  vTagDetectors, tagdetectutils::ArucoTag &  stArucoTarget, tagdetectutils::ArucoTag &  stTorchTarget, const int  nTargetTagID = static_cast<int>(manifest::Autonomy::AUTONOMYWAYPOINTTYPES::ANY)  )

inline

Identify a target marker in the rover's vision, using OpenCV detection.

Note

If multiple markers are detected the closest one will be chosen as the target.

Parameters

vTagDetectors	- The vector of tag detectors to use for detection.
stArucoTarget	- The detected target marker from OpenCV.
stTorchTarget	- The detected target marker from Torch.
nTargetTagID	- The ID of the target tag to identify. If -1, the closest tag will be chosen.

Returns

int - The total number of tags currently detected.

Author

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

Date

2025-04-04

    {
        // Create instance variables.
        std::vector<tagdetectutils::ArucoTag> vDetectedArucoTags;
        tagdetectutils::ArucoTag stArucoBestTag;
        tagdetectutils::ArucoTag stTorchBestTag;
        std::string szIdentifiedTags = "";
 
        // Initialize best percentages to 0 so the first valid tag always wins
        double dBestArucoAreaPercentage = 0.0;
        double dBestTorchAreaPercentage = 0.0;
 
        // Get the current time
        std::chrono::system_clock::time_point tmCurrentTime = std::chrono::system_clock::now();
 
        // Load all detected tags in the rover's vision.
        LoadDetectedTags(vDetectedArucoTags, vTagDetectors);
 
        // Find the best tag from the Aruco tags.
        for (const tagdetectutils::ArucoTag& stCandidate : vDetectedArucoTags)
        {
            // Calculate the total age of the tag.
            double dTagTotalAge = std::fabs(std::chrono::duration_cast<std::chrono::milliseconds>(tmCurrentTime - stCandidate.tmCreation).count() / 1000.0);
 
            // Safety check for bounding box pointer
            if (stCandidate.pBoundingBox == nullptr)
            {
                continue;
            }
 
            // Calculate what percentage of the screen the tag takes up.
            double dArea           = stCandidate.pBoundingBox->area();
            double dAreaPercentage = (dArea / (stCandidate.cvImageResolution.width * stCandidate.cvImageResolution.height)) * 100.0;
 
            // Check if the tag meets the minimum thresholds.
            if (dAreaPercentage < constants::BBOX_MIN_SCREEN_PERCENTAGE || dTagTotalAge < constants::BBOX_MIN_LIFETIME_THRESHOLD)
            {
                continue;
            }
 
            // --- OpenCV Tag Logic ---
            if (stCandidate.eDetectionMethod == tagdetectutils::TagDetectionMethod::eOpenCV)
            {
                szIdentifiedTags += "\tArUco ID: " + std::to_string(stCandidate.nID) + " Tag Age: " + std::to_string(dTagTotalAge) +
                                    "s Tag Screen Percentage: " + std::to_string(dAreaPercentage) + "%\n";
 
                // Ensure it matches the requested ID (or we accept ANY ID)
                if (stCandidate.nID == nTargetTagID || nTargetTagID == static_cast<int>(manifest::Autonomy::AUTONOMYWAYPOINTTYPES::ANY))
                {
                    // Prioritize the tag that takes up the most screen area
                    if (dAreaPercentage > dBestArucoAreaPercentage)
                    {
                        stArucoBestTag           = stCandidate;
                        dBestArucoAreaPercentage = dAreaPercentage;
                    }
                }
            }
            // --- Torch Tag Logic ---
            else if (stCandidate.eDetectionMethod == tagdetectutils::TagDetectionMethod::eTorch)
            {
                szIdentifiedTags += "\tTorch Class: " + stCandidate.szClassName + " Tag Age: " + std::to_string(dTagTotalAge) +
                                    "s Tag Screen Percentage: " + std::to_string(dAreaPercentage) + "%\n";
 
                // Prioritize the tag that takes up the most screen area
                if (dAreaPercentage > dBestTorchAreaPercentage)
                {
                    stTorchBestTag           = stCandidate;
                    dBestTorchAreaPercentage = dAreaPercentage;
                }
            }
        }
 
        // Only print the identified tags if there are any.
        if (stArucoBestTag.nID != -1 || stTorchBestTag.dConfidence != 0.0)
        {
            LOG_DEBUG(logging::g_qSharedLogger, "TagDetectionChecker: Identified tags:\n{}", szIdentifiedTags);
        }
 
        // Set the target tag to the best tag.
        stArucoTarget = stArucoBestTag;
        stTorchTarget = stTorchBestTag;
 
        return static_cast<int>(vDetectedArucoTags.size());
    }

Here is the call graph for this function:

Here is the caller graph for this function: