statemachine::ApproachingMarkerState Class Reference

The ApproachingMarkerState class implements the Approaching Marker state for the Autonomy State Machine. More...

#include <ApproachingMarkerState.h>

Inheritance diagram for statemachine::ApproachingMarkerState:

Collaboration diagram for statemachine::ApproachingMarkerState:

Public Member Functions
	ApproachingMarkerState ()
	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 Member Functions
bool	IdentifyTargetArucoMarker (arucotag::ArucoTag &stTarget)
	Identify a target marker in the rover's vision, using OpenCV detection.
bool	IdentifyTargetTensorflowMarker (tensorflowtag::TensorflowTag &stTarget)
	Identify a target marker in the rover's vision, using Tensorflow detection.

Private Attributes
bool	m_bInitialized
States	m_eTriggeringState
int	m_nNumDetectionAttempts
int	m_nTargetTagID
bool	m_bDetected
arucotag::ArucoTag	m_stTargetTagAR
tensorflowtag::TensorflowTag	m_stTargetTagTF
double	m_dLastTargetHeading
double	m_dLastTargetDistance
std::vector< TagDetector * >	m_vTagDetectors

Detailed Description

The ApproachingMarkerState class implements the Approaching Marker state for the Autonomy State Machine.

Author

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

Date

2024-01-17

Constructor & Destructor Documentation

ApproachingMarkerState()

statemachine::ApproachingMarkerState::ApproachingMarkerState

(

)

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::eApproachingMarker)
    {
        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

IdentifyTargetArucoMarker()

bool statemachine::ApproachingMarkerState::IdentifyTargetArucoMarker ( arucotag::ArucoTag &  stTarget )

private

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

tTarget

- Reference to store the tag identified as the target.

Returns

true - A target marker was identified.

false - A target marker was not identified.

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-02-29

    {
        // Load all detected tags in the rover's vision.
        std::vector<arucotag::ArucoTag> vDetectedArucoTags;
        std::vector<tensorflowtag::TensorflowTag> vDetectedTensorflowTags;
        tagdetectutils::LoadDetectedTags(vDetectedArucoTags, vDetectedTensorflowTags, m_vTagDetectors);
 
        arucotag::ArucoTag stBestTag;
        stBestTag.dStraightLineDistance = std::numeric_limits<double>::max();
        stBestTag.nID                   = -1;
 
        // Create string to store detected tags in.
        std::string szIdentifiedTags = "";
 
        // Select the tag that is the closest to the rover's current position.
        for (const arucotag::ArucoTag& stCandidate : vDetectedArucoTags)
        {
            szIdentifiedTags += "\tID: " + std::to_string(stCandidate.nID) + " Hits: " + std::to_string(stCandidate.nHits) + "\n";
 
            if (stCandidate.dStraightLineDistance < stBestTag.dStraightLineDistance)
            {
                stBestTag = stCandidate;
            }
        }
 
        // A tag was found.
        if (stBestTag.nID >= 0)
        {
            // TODO: Change to a Debug Statement after we confirm it works.
            LOG_INFO(logging::g_qSharedLogger, "ApproachingMarkerState: Detected Tags: \n{}", szIdentifiedTags);
            LOG_INFO(logging::g_qSharedLogger, "ApproachingMarkerState: Best Tag: \n\tID: {} Hits: {}", stBestTag.nID, stBestTag.nHits);
 
            // Save it to the passed in reference.
            stTarget = stBestTag;
            return true;
        }
        // No target tag was found.
        else
        {
            // TODO: Change to a Debug Statement after we confirm it works.
            LOG_INFO(logging::g_qSharedLogger, "ApproachingMarkerState: No Tag Detected!");
            return false;
        }
    }

Here is the call graph for this function:

Here is the caller graph for this function:

IdentifyTargetTensorflowMarker()

bool statemachine::ApproachingMarkerState::IdentifyTargetTensorflowMarker ( tensorflowtag::TensorflowTag &  stTarget )

private

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

Note

If multiple markers are detected the closest one will be chosen as the target. Also all tags must have a confidence of at least APPROACH_MARKER_TF_CONFIDENCE_THRESHOLD.

Parameters

tTarget

- Reference to store the tag identified as the target.

Returns

true - A target marker was identified.

false - A target marker was not identified.

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-02-29

    {
        // Load all detected tags in the rover's vision.
        std::vector<arucotag::ArucoTag> vDetectedArucoTags;
        std::vector<tensorflowtag::TensorflowTag> vDetectedTensorflowTags;
        tagdetectutils::LoadDetectedTags(vDetectedArucoTags, vDetectedTensorflowTags, m_vTagDetectors);
 
        tensorflowtag::TensorflowTag stBestTag;
        stBestTag.dStraightLineDistance = std::numeric_limits<double>::max();
        bool bTagIdentified             = false;
        // stBestTag.nID                   = -1;
 
        // Select the tag that is the closest to the rover's current position and above the confidence threshold.
        for (const tensorflowtag::TensorflowTag& stCandidate : vDetectedTensorflowTags)
        {
            if (stCandidate.dStraightLineDistance < stBestTag.dStraightLineDistance && stCandidate.dConfidence >= constants::APPROACH_MARKER_TF_CONFIDENCE_THRESHOLD)
            {
                stBestTag = stCandidate;
 
                // Update tag found toggle.
                bTagIdentified = true;
            }
        }
 
        // Check if a tag has been identified.
        if (bTagIdentified)
        {
            // Save it to the passed in reference.
            stTarget = stBestTag;
        }
 
        return bTagIdentified;
    }

Here is the call graph for this function:

Here is the caller graph for this function:

Start()

void statemachine::ApproachingMarkerState::Start ( )

overrideprotectedvirtual

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

Author

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

Date

2024-01-17

Reimplemented from statemachine::State.

    {
        // Schedule the next run of the state's logic
        LOG_INFO(logging::g_qSharedLogger, "ApproachingMarkerState: Scheduling next run of state logic.");
 
        // Initialize member variables.
        m_nNumDetectionAttempts = 0;
        m_nTargetTagID          = -1;
        m_bDetected             = false;
        m_dLastTargetHeading    = 0;
        m_dLastTargetDistance   = 0;
 
        // Store the state that got stuck and triggered a MarkerSeen event.
        m_eTriggeringState = globals::g_pStateMachineHandler->GetPreviousState();
 
        // Get tag detectors.
        m_vTagDetectors = {globals::g_pTagDetectionHandler->GetTagDetector(TagDetectionHandler::TagDetectors::eHeadMainCam),
                           globals::g_pTagDetectionHandler->GetTagDetector(TagDetectionHandler::TagDetectors::eFrameLeftCam),
                           globals::g_pTagDetectionHandler->GetTagDetector(TagDetectionHandler::TagDetectors::eFrameRightCam)};
    }

Here is the call graph for this function:

Here is the caller graph for this function:

Exit()

void statemachine::ApproachingMarkerState::Exit ( )

overrideprotectedvirtual

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

Author

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

Date

2024-01-17

Reimplemented from statemachine::State.

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

Here is the caller graph for this function:

Run()

void statemachine::ApproachingMarkerState::Run ( )

overridevirtual

Run the state machine. Returns the next state.

Author

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

Date

2024-01-17

Implements statemachine::State.

    {
        // Submit logger message.
        LOG_DEBUG(logging::g_qSharedLogger, "ApproachingMarkerState: Running state-specific behavior.");
 
        // Create instance variables.
        bool bDetectedTagAR = false;    // Was the tag detected through OpenCV.
        bool bDetectedTagTF = false;    // Was the tag detected through Tensorflow.
 
        // Get the current rover pose.
        geoops::RoverPose stCurrentRoverPose = globals::g_pWaypointHandler->SmartRetrieveRoverPose();
 
        // If a target hasn't been identified yet attempt to find a target tag in the rover's vision.
        if (!m_bDetected && m_nNumDetectionAttempts < constants::APPROACH_MARKER_DETECT_ATTEMPTS_LIMIT)
        {
            // Attempt to identify the target with OpenCV.
            // While OpenCV struggles to find tags, the tags it does find are much more reliable compared to TensorFlow.
            bDetectedTagAR = IdentifyTargetArucoMarker(m_stTargetTagAR);
            if (bDetectedTagAR)
            {
                // Save the identified tag's ID.
                m_nTargetTagID          = m_stTargetTagAR.nID;
                m_bDetected             = true;
                m_nNumDetectionAttempts = 0;
            }
            else
            {
                // If OpenCV fails to find the tag rely on the TensorFlow vision algorithms to identify it.
                bDetectedTagTF = IdentifyTargetTensorflowMarker(m_stTargetTagTF);
                if (bDetectedTagTF)
                {
                    // Save the identified tag's ID.
                    // NOTE: Commented this out since TensorflowTag no longer has ID.
                    // m_nTargetTagID          = m_stTargetTagTF.nID;
                    m_bDetected             = true;
                    m_nNumDetectionAttempts = 0;
                }
            }
 
            // Both OpenCV & TensorFlow failed to identify a target tag.
            if (!m_bDetected)
            {
                ++m_nNumDetectionAttempts;
            }
 
            return;
        }
        // A target hasn't been identified and the amount of attempts has exceeded the limit.
        else if (!m_bDetected)
        {
            // Abort approaching marker.
            globals::g_pStateMachineHandler->HandleEvent(Event::eMarkerUnseen);
            return;
        }
 
        // Attempt to find the target marker in OpenCV.
        bDetectedTagAR = tagdetectutils::FindArucoTagByID(m_nTargetTagID, m_stTargetTagAR, m_vTagDetectors);
        // LEAD: Commented this out since TensorflowTag no longer has ID.
        // if (!bDetectedTagAR)
        // {
        //     // Attempt to find the target marker in TensorFlow.
        //     bDetectedTagTF = tagdetectutils::FindTensorflowTagByID(m_nTargetTagID, m_stTargetTagTF, m_vTagDetectors);
        // }
 
        // The target marker wasn't found.
        if (!bDetectedTagAR && !bDetectedTagTF)
        {
            ++m_nNumDetectionAttempts;
        }
        // The target marker was found.
        else
        {
            m_nNumDetectionAttempts = 0;
        }
 
        // If we have made too many consecutive failed detection attempts
        // inform the statemachine the marker has been lost.
        if (m_nNumDetectionAttempts >= constants::APPROACH_MARKER_DETECT_ATTEMPTS_LIMIT)
        {
            globals::g_pStateMachineHandler->HandleEvent(Event::eMarkerUnseen);
            return;
        }
 
        // Get the current absolute heading of the rover.
        double dCurrHeading = stCurrentRoverPose.GetCompassHeading();
 
        // Find the target's heading and distance with respect to the rover's current position.
        double dTargetHeading;
        double dTargetDistance;
 
        if (bDetectedTagAR)
        {
            dTargetHeading  = numops::InputAngleModulus<double>(dCurrHeading + m_stTargetTagAR.dYawAngle, 0, 360);
            dTargetDistance = m_stTargetTagAR.dStraightLineDistance;
        }
        else if (bDetectedTagTF)
        {
            dTargetHeading  = numops::InputAngleModulus<double>(dCurrHeading + m_stTargetTagTF.dYawAngle, 0, 360);
            dTargetDistance = m_stTargetTagTF.dStraightLineDistance;
        }
        // Use the last recorded heading and distance.
        else
        {
            dTargetHeading  = m_dLastTargetHeading;
            dTargetDistance = m_dLastTargetDistance;
        }
        // Save the found heading and distance.
        m_dLastTargetHeading  = dTargetHeading;
        m_dLastTargetDistance = dTargetDistance;
 
        // Only print out every so often.
        static bool bAlreadyPrinted = false;
        if ((std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count() % 5) == 0 && !bAlreadyPrinted)
        {
            // Submit logger message.
            LOG_INFO(logging::g_qSharedLogger,
                     "ApproachingMarkerState: Rover is {} meters from the marker. Minimum Distance is {}.",
                     dTargetDistance,
                     constants::APPROACH_MARKER_PROXIMITY_THRESHOLD);
            // Set toggle.
            bAlreadyPrinted = true;
        }
        else if ((std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count() % 5) != 0 && bAlreadyPrinted)
        {
            // Reset toggle.
            bAlreadyPrinted = false;
        }
 
        // If we are close enough to the target inform the state machine we have reached the marker.
        if (dTargetDistance < constants::APPROACH_MARKER_PROXIMITY_THRESHOLD)
        {
            globals::g_pStateMachineHandler->HandleEvent(Event::eReachedMarker);
            return;
        }
 
        // Move the rover to the target's estimated position.
        diffdrive::DrivePowers stDrivePowers = globals::g_pDriveBoard->CalculateMove(constants::APPROACH_MARKER_MOTOR_POWER,
                                                                                     dTargetHeading,
                                                                                     dCurrHeading,
                                                                                     diffdrive::DifferentialControlMethod::eArcadeDrive);
        globals::g_pDriveBoard->SendDrive(stDrivePowers);
 
        return;
    }

Here is the call graph for this function:

TriggerEvent()

States statemachine::ApproachingMarkerState::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

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

Date

2024-01-17

Implements statemachine::State.

    {
        // Create instance variables.
        States eNextState       = States::eApproachingMarker;
        bool bCompleteStateExit = true;
 
        switch (eEvent)
        {
            case Event::eReachedMarker:
            {
                // Submit logger message.
                LOG_INFO(logging::g_qSharedLogger, "ApproachingMarkerState: Handling ReachedMarker event.");
                // Send multimedia command to update state display.
                globals::g_pMultimediaBoard->SendLightingState(MultimediaBoard::MultimediaBoardLightingState::eReachedGoal);
                // Pop old waypoint out of queue.
                globals::g_pWaypointHandler->PopNextWaypoint();
                // Clear saved search pattern state.
                globals::g_pStateMachineHandler->ClearSavedState(States::eSearchPattern);
                // Submit logger message.
                LOG_NOTICE(logging::g_qSharedLogger, "ApproachingMarkerState: Cleared old search pattern state from saved states.");
                // Change states.
                eNextState = States::eIdle;
                break;
            }
            case Event::eStart:
            {
                // Submit logger message.
                LOG_INFO(logging::g_qSharedLogger, "ApproachingMarkerState: Handling Start event.");
                // Send multimedia command to update state display.
                globals::g_pMultimediaBoard->SendLightingState(MultimediaBoard::MultimediaBoardLightingState::eAutonomy);
                break;
            }
            case Event::eMarkerUnseen:
            {
                // Submit logger message.
                LOG_INFO(logging::g_qSharedLogger, "ApproachingMarkerState: Handling MarkerUnseen event.");
                // Change states.
                eNextState = m_eTriggeringState;
                break;
            }
            case Event::eAbort:
            {
                // Submit logger message.
                LOG_INFO(logging::g_qSharedLogger, "ApproachingMarkerState: Handling Abort event.");
                // Send multimedia command to update state display.
                globals::g_pMultimediaBoard->SendLightingState(MultimediaBoard::MultimediaBoardLightingState::eAutonomy);
                // Change state.
                eNextState = States::eIdle;
                break;
            }
            default:
            {
                LOG_WARNING(logging::g_qSharedLogger, "ApproachingMarkerState: Handling unknown event.");
                eNextState = States::eIdle;
                break;
            }
        }
 
        if (eNextState != States::eApproachingMarker)
        {
            LOG_INFO(logging::g_qSharedLogger, "ApproachingMarkerState: 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/ApproachingMarkerState.h
• src/states/ApproachingMarkerState.cpp