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 Tensorflow 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

vDetectedObject	- 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;
 
        // 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]));
            }
        }
 
        // Ensure all requests have been fulfilled.
        // Then transfer objects from the buffer to vDetectedObjects for the user to access.
        for (size_t siIdx = 0; siIdx < vDetectedObjectsFuture.size(); ++siIdx)
        {
            // Wait for the request to be fulfilled.
            vDetectedObjectsFuture[siIdx].get();
 
            // 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 = "";
 
        // 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);
            // Calculate the total object area.
            double dArea = stCandidate.pBoundingBox->area();
            // Calculate what percentage of the screen the object takes up.
            double dAreaPercentage = (dArea / (stCandidate.cvImageResolution.width * stCandidate.cvImageResolution.height)) * 100.0;
 
            // If the distance of the object is not greater than 0, skip it.
            if (stCandidate.dStraightLineDistance <= 0.0)
            {
                continue;
            }
 
            // 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 requirements.
                if (dAreaPercentage < constants::BBOX_MIN_SCREEN_PERCENTAGE || dObjectTotalAge < constants::BBOX_MIN_LIFETIME_THRESHOLD)
                {
                    continue;
                }
 
                // Check other object requirements.
                if (dArea > stBestObject.pBoundingBox->area())
                {
                    // Set the target object to the detected object.
                    stBestObject = stCandidate;
                }
            }
        }
 
        // 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 Tensorflow 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;
 
        // 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]));
            }
        }
 
        // Ensure all requests have been fulfilled.
        // Then transfer tags from the buffer to vDetectedArucoTags and vDetectedTensorflowTags for the user to access.
        for (size_t siIdx = 0; siIdx < vDetectedArucoTagsFuture.size(); ++siIdx)
        {
            // Wait for the request to be fulfilled.
            vDetectedArucoTagsFuture[siIdx].get();
 
            // Loop through the detected Aruco tags and add them to the vDetectedArucoTags vector.
            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 = "";
 
        // 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);
            // Calculate the total tag area.
            double dArea = stCandidate.pBoundingBox->area();
            // Calculate what percentage of the screen the tag takes up.
            double dAreaPercentage = (dArea / (stCandidate.cvImageResolution.width * stCandidate.cvImageResolution.height)) * 100.0;
 
            // If the distance of the tag is not greater than 0, skip it.
            if (stCandidate.dStraightLineDistance <= 0.0)
            {
                continue;
            }
 
            // Check the tag detection method type.
            if (stCandidate.eDetectionMethod == tagdetectutils::TagDetectionMethod::eOpenCV)
            {
                // Assemble the identified tags string.
                szIdentifiedTags += "\tArUco ID: " + std::to_string(stCandidate.nID) + " Tag Age: " + std::to_string(dTagTotalAge) +
                                    "s Tag Screen Percentage: " + std::to_string(dAreaPercentage) + "%\n";
                // Check if the tag is best.
                if (stCandidate.nID == nTargetTagID || static_cast<int>(manifest::Autonomy::AUTONOMYWAYPOINTTYPES::ANY))
                {
                    // Check if the tag meets the requirements.
                    if (dAreaPercentage < constants::BBOX_MIN_SCREEN_PERCENTAGE || dTagTotalAge < constants::BBOX_MIN_LIFETIME_THRESHOLD)
                    {
                        continue;
                    }
 
                    // Check other tag requirements.
                    if (dArea > stArucoBestTag.pBoundingBox->area())
                    {
                        // Set the target tag to the detected tag.
                        stArucoBestTag = stCandidate;
                    }
                }
            }
            else if (stCandidate.eDetectionMethod == tagdetectutils::TagDetectionMethod::eTorch)
            {
                // Assemble the identified tags string.
                szIdentifiedTags += "\tTorch Class: " + stCandidate.szClassName + " Tag Age: " + std::to_string(dTagTotalAge) +
                                    "s Tag Screen Percentage: " + std::to_string(dAreaPercentage) + "%\n";
                // Check if the tag meets the requirements.
                if (dAreaPercentage < constants::BBOX_MIN_SCREEN_PERCENTAGE || dTagTotalAge < constants::BBOX_MIN_LIFETIME_THRESHOLD)
                {
                    continue;
                }
 
                // Check other tag requirements.
                if (dArea > stTorchBestTag.pBoundingBox->area())
                {
                    // Set the target tag to the detected tag.
                    stTorchBestTag = stCandidate;
                }
            }
        }
 
        // Only print the identified tags if there are any.
        if (stArucoBestTag.nID != -1 || stTorchBestTag.dConfidence != 0.0)
        {
            // Submit logger message.
            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: