statemachine::VerifyingObjectState Class Reference

The VerifyingObjectState class implements the Verifying Object state for the Autonomy State Machine. More...

#include <VerifyingObjectState.h>

Inheritance diagram for statemachine::VerifyingObjectState:

Collaboration diagram for statemachine::VerifyingObjectState:

Public Member Functions
	VerifyingObjectState ()
	Accessor for the State private member. Returns the state as a string.
void	Run () override
	Run the state machine. Returns the next state.
States	TriggerEvent (Event eEvent) override
	Trigger an event in the state machine. Returns the next state.
Public Member Functions inherited from statemachine::State
	State (States eState)
	Construct a new State object.
virtual	~State ()=default
	Destroy the State object.
States	GetState () const
	Accessor for the State private member.
virtual std::string	ToString () const
	Accessor for the State private member. Returns the state as a string.
virtual bool	operator== (const State &other) const
	Checks to see if the current state is equal to the passed state.
virtual bool	operator!= (const State &other) const
	Checks to see if the current state is not equal to the passed state.

Protected Member Functions
void	Start () override
	This method is called when the state is first started. It is used to initialize the state.
void	Exit () override
	This method is called when the state is exited. It is used to clean up the state.

Private Attributes
bool	m_bInitialized
geoops::Waypoint	m_stGoalWaypoint
objectdetectutils::Object	m_stBestObject
std::vector< std::shared_ptr< ObjectDetector > >	m_vObjectDetectors
std::chrono::system_clock::time_point	m_tmObjectVerificationStartTime
std::chrono::system_clock::time_point	m_tmObjectLastSeenTime
std::chrono::system_clock::time_point	m_tmLastRunTime
double	m_dTotalValidTime

Detailed Description

The VerifyingObjectState class implements the Verifying Object state for the Autonomy State Machine.

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

Constructor & Destructor Documentation

VerifyingObjectState()

statemachine::VerifyingObjectState::VerifyingObjectState

(

)

Accessor for the State private member. Returns the state as a string.

Returns

std::string - The current state as a string.

Author

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

Date

2024-01-17

                                               : State(States::eVerifyingObject)
    {
        LOG_INFO(logging::g_qConsoleLogger, "Entering State: {}", ToString());
 
        m_bInitialized = false;
 
        if (!m_bInitialized)
        {
            Start();
            m_bInitialized = true;
        }
    }

Here is the call graph for this function:

Member Function Documentation

Start()

void statemachine::VerifyingObjectState::Start ( )

overrideprotectedvirtual

This method is called when the state is first started. It is used to initialize the state.

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

Reimplemented from statemachine::State.

    {
        // Schedule the next run of the state's logic
        LOG_INFO(logging::g_qSharedLogger, "VerifyingObjectState: Scheduling next run of state logic.");
 
        // Initialize member variables.
        m_stGoalWaypoint                = globals::g_pWaypointHandler->PeekNextWaypoint();
        m_tmObjectVerificationStartTime = std::chrono::system_clock::now();
        m_tmObjectLastSeenTime          = std::chrono::system_clock::now();
 
        // Initialize time-based hit tracking
        m_tmLastRunTime   = std::chrono::system_clock::now();
        m_dTotalValidTime = 0.0;
 
        // Get object detectors.
        m_vObjectDetectors = {globals::g_pObjectDetectionHandler->GetObjectDetector(ObjectDetectionHandler::ObjectDetectors::eHeadMainCam),
                              globals::g_pObjectDetectionHandler->GetObjectDetector(ObjectDetectionHandler::ObjectDetectors::eRearCam)};
    }

Here is the call graph for this function:

Here is the caller graph for this function:

Exit()

void statemachine::VerifyingObjectState::Exit ( )

overrideprotectedvirtual

This method is called when the state is exited. It is used to clean up the state.

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

Reimplemented from statemachine::State.

    {
        // Clean up the state before exiting
        LOG_INFO(logging::g_qSharedLogger, "VerifyingObjectState: Exiting state.");
    }

Here is the caller graph for this function:

Run()

void statemachine::VerifyingObjectState::Run ( )

overridevirtual

Run the state machine. Returns the next state.

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

Implements statemachine::State.

    {
        LOG_DEBUG(logging::g_qSharedLogger, "VerifyingObjectState: Running state-specific behavior.");
 
        // IMPORTANT: Ensure the rover is completely stopped to avoid motion blur during verification.
        globals::g_pDriveBoard->SendStop();
 
        // 1. Calculate delta time since the last execution of Run()
        std::chrono::system_clock::time_point tmCurrentTime = std::chrono::system_clock::now();
        double dDeltaTime                                   = std::chrono::duration_cast<std::chrono::milliseconds>(tmCurrentTime - m_tmLastRunTime).count() / 1000.0;
        m_tmLastRunTime                                     = tmCurrentTime;    // Reset for the next loop
 
        // 2. Identify target object.
        objectdetectutils::Object stBestObject;
        statemachine::IdentifyTargetObject(m_vObjectDetectors, stBestObject, m_stGoalWaypoint.eType);
 
        // 3. Update Time-Based Hit Rate
        if (stBestObject.dConfidence > 0.0)
        {
            // Add the time that elapsed during this valid frame to our total valid time
            m_dTotalValidTime += dDeltaTime;
 
            m_stBestObject         = stBestObject;    // Save the best object for the snapshot later
            m_tmObjectLastSeenTime = tmCurrentTime;
        }
 
        double dElapsedTime       = std::chrono::duration_cast<std::chrono::milliseconds>(tmCurrentTime - m_tmObjectVerificationStartTime).count() / 1000.0;
        double dTimeSinceLastSeen = std::chrono::duration_cast<std::chrono::milliseconds>(tmCurrentTime - m_tmObjectLastSeenTime).count() / 1000.0;
 
        // 4. Fast-Fail if object is completely lost
        if (dTimeSinceLastSeen > constants::APPROACH_OBJECT_LOST_BUFFER_TIME)
        {
            LOG_WARNING(logging::g_qSharedLogger, "VerifyingObjectState: Lost visual entirely. Fast failing verification.");
            globals::g_pStateMachineHandler->HandleEvent(Event::eVerifyingFailed);
            return;
        }
 
        // 5. Final Evaluation based on elapsed verification time
        if (dElapsedTime >= constants::APPROACH_OBJECT_VERIFY_TIME)
        {
            // Calculate the percentage of time the object was validly tracked
            double dTimeHitRate = m_dTotalValidTime / constants::APPROACH_OBJECT_VERIFY_TIME;
 
            if (dTimeHitRate >= constants::APPROACH_OBJECT_REQUIRED_TIME_HIT_RATE)
            {
                LOG_NOTICE(logging::g_qSharedLogger,
                           "VerifyingObjectState: SUCCESS! Confirmed visually for {:.2f}s ({:.2f}% of the required window).",
                           m_dTotalValidTime,
                           dTimeHitRate * 100.0);
                globals::g_pStateMachineHandler->HandleEvent(Event::eVerifyingComplete);
            }
            else
            {
                LOG_WARNING(logging::g_qSharedLogger,
                            "VerifyingObjectState: FALSE POSITIVE. Confirmed visually for only {:.2f}s ({:.2f}% of window).",
                            m_dTotalValidTime,
                            dTimeHitRate * 100.0);
                globals::g_pStateMachineHandler->HandleEvent(Event::eVerifyingFailed);
            }
        }
    }

Here is the call graph for this function:

TriggerEvent()

States statemachine::VerifyingObjectState::TriggerEvent ( Event  eEvent )

overridevirtual

Trigger an event in the state machine. Returns the next state.

Parameters

eEvent

- The event to trigger.

Returns

std::shared_ptr<State> - The next state.

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

Implements statemachine::State.

    {
        // Create instance variables.
        States eNextState       = States::eVerifyingObject;
        bool bCompleteStateExit = true;
 
        switch (eEvent)
        {
            case Event::eStart:
            {
                // Submit logger message.
                LOG_INFO(logging::g_qSharedLogger, "VerifyingObjectState: Handling Start event.");
                // Send multimedia command to update state display.
                globals::g_pMultimediaBoard->SendLightingState(MultimediaBoard::MultimediaBoardLightingState::eAutonomy);
                break;
            }
            case Event::eVerifyingComplete:
            {
                // Submit logger message.
                LOG_INFO(logging::g_qSharedLogger, "VerifyingObjectState: Handling Verifying Complete event.");
                // Send multimedia command to update state display.
                globals::g_pMultimediaBoard->SendLightingState(MultimediaBoard::MultimediaBoardLightingState::eReachedGoal);
 
                // Loop through the detectors vector and find which ones UUID matches the winning tag's UUID.
                // If a match is found, request the snapshot from that detector and save it to disk with a unique filename.
                cv::Mat cvSnapshot;
                for (const std::shared_ptr<ObjectDetector>& pObjectDetector : m_vObjectDetectors)
                {
                    if (pObjectDetector->GetThreadUUID() == m_stBestObject.szDetectorUUID)
                    {
                        std::future<bool> fuFrame = pObjectDetector->RequestLastGoodDetectionOverlayFrame(cvSnapshot);
                        if (!fuFrame.get())
                        {
                            LOG_WARNING(logging::g_qSharedLogger, "VerifyingObjectState: Failed to request detection overlay frame.");
                        }
                        break;
                    }
                }
 
                // Check if the snapshot is empty.
                if (!cvSnapshot.empty())
                {
                    std::string szLogDir = logging::g_szLoggingOutputPath + "/detections/";
                    if (!std::filesystem::exists(szLogDir))
                    {
                        std::filesystem::create_directories(szLogDir);
                    }
 
                    // Create a unique filename using the current timestamp
                    std::string szTimestamp = timeops::GetTimestamp();
                    std::string szFilename  = szLogDir + "object_" + szTimestamp + ".png";
 
                    // Save the image to the disk
                    bool bSuccess = cv::imwrite(szFilename, cvSnapshot);
 
                    if (bSuccess)
                    {
                        LOG_INFO(logging::g_qSharedLogger, "VerifyingObjectState: Saved detection snapshot to {}", szFilename);
                    }
                    else
                    {
                        LOG_ERROR(logging::g_qSharedLogger, "VerifyingObjectState: Failed to write snapshot to disk.");
                    }
                }
                else
                {
                    LOG_WARNING(logging::g_qSharedLogger, "VerifyingObjectState: Overlay frame was empty. No snapshot taken.");
                }
 
                // Pop old waypoint out of queue.
                globals::g_pWaypointHandler->PopNextWaypoint();
                // Clear saved states.
                globals::g_pStateMachineHandler->ClearSavedStates();
                // Submit logger message.
                LOG_NOTICE(logging::g_qSharedLogger, "VerifyingObjectState: Cleared old saved states.");
                // Change state.
                eNextState = States::eIdle;
                break;
            }
            case Event::eVerifyingFailed:
            {
                // Submit logger message.
                LOG_INFO(logging::g_qSharedLogger, "VerifyingObjectState: Handling Verifying Failed/Object Unseen event.");
                // Send multimedia command to update state display.
                globals::g_pMultimediaBoard->SendLightingState(MultimediaBoard::MultimediaBoardLightingState::eAutonomy);
                // Recall the previous state.
                eNextState = globals::g_pStateMachineHandler->GetPreviousState();
                break;
            }
            case Event::eStuck:
            {
                LOG_INFO(logging::g_qSharedLogger, "NavigatingState: Handling Stuck event.");
                eNextState = States::eStuck;
                break;
            }
            case Event::eAbort:
            {
                // Submit logger message.
                LOG_INFO(logging::g_qSharedLogger, "VerifyingObjectState: Handling Abort event.");
                // Send multimedia command to update state display.
                globals::g_pMultimediaBoard->SendLightingState(MultimediaBoard::MultimediaBoardLightingState::eOff);
                // Change state.
                eNextState = States::eIdle;
                break;
            }
            default:
            {
                LOG_WARNING(logging::g_qSharedLogger, "VerifyingObjectState: Handling unknown event.");
                eNextState = States::eIdle;
                break;
            }
        }
 
        if (eNextState != States::eVerifyingObject)
        {
            LOG_INFO(logging::g_qSharedLogger, "VerifyingObjectState: Transitioning to {} State.", StateToString(eNextState));
 
            // Exit the current state
            if (bCompleteStateExit)
            {
                Exit();
            }
        }
 
        return eNextState;
    }

Here is the call graph for this function:

---

The documentation for this class was generated from the following files:

• src/states/VerifyingObjectState.h
• src/states/VerifyingObjectState.cpp