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.

Private 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
std::vector< std::shared_ptr< TagDetector > >	m_vTagDetectors
statemachine::TimeIntervalBasedStuckDetector	m_StuckDetector
States	m_eTriggeringState
bool	m_bInitialized
geoops::Waypoint	m_stGoalWaypoint
std::unique_ptr< logging::graphing::PathTracer >	m_pRoverPathPlot

Additional Inherited Members

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;
 
        m_StuckDetector  = statemachine::TimeIntervalBasedStuckDetector(constants::STUCK_CHECK_ATTEMPTS,
                                                                       constants::STUCK_CHECK_INTERVAL,
                                                                       constants::STUCK_CHECK_VEL_THRESH,
                                                                       constants::STUCK_CHECK_ROT_THRESH);
        m_pRoverPathPlot = std::make_unique<logging::graphing::PathTracer>("ApproachingMarkerRoverPath");
 
        if (!m_bInitialized)
        {
            Start();
            m_bInitialized = true;
        }
    }

Here is the call graph for this function:

Member Function Documentation

Start()

void statemachine::ApproachingMarkerState::Start ( )

overrideprivatevirtual

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_stGoalWaypoint = globals::g_pWaypointHandler->PeekNextWaypoint();
 
        // Store the state that got stuck and triggered a MarkerSeen event.
        m_eTriggeringState = globals::g_pStateMachineHandler->GetPreviousState();
 
        // Add the search and rover path layers to the plot.
        m_pRoverPathPlot->CreateDotLayer("DetectedTags", "blue");
        m_pRoverPathPlot->CreateDotLayer("FinalTag", "green");
        m_pRoverPathPlot->CreatePathLayer("RoverPath", "-k");
 
        // Get tag detectors.
        m_vTagDetectors = {globals::g_pTagDetectionHandler->GetTagDetector(TagDetectionHandler::TagDetectors::eHeadMainCam)};
    }

Here is the call graph for this function:

Here is the caller graph for this function:

Exit()

void statemachine::ApproachingMarkerState::Exit ( )

overrideprivatevirtual

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

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.

    {
        // Submit logger message.
        LOG_DEBUG(logging::g_qSharedLogger, "ApproachingMarkerState: Running state-specific behavior.");
 
        // Get the current rover pose.
        geoops::RoverPose stCurrentRoverPose = globals::g_pStateMachineHandler->SmartRetrieveRoverPose();
 
        // Add the current rover pose to the path plot.
        m_pRoverPathPlot->AddPathPoint(stCurrentRoverPose.GetUTMCoordinate(), "RoverPath");
 
        // Check Rover radius from marker waypoint.
        geoops::GeoMeasurement stCurrentMeasurement = geoops::CalculateGeoMeasurement(m_stGoalWaypoint.GetGPSCoordinate(), stCurrentRoverPose.GetGPSCoordinate());
        if (stCurrentMeasurement.dDistanceMeters > m_stGoalWaypoint.dRadius)
        {
            // Submit logger message.
            LOG_WARNING(logging::g_qSharedLogger,
                        "ApproachingMarkerState: Rover is too far from the original waypoint! Waypoint radius is {} meters, current distance is {} meters.",
                        m_stGoalWaypoint.dRadius,
                        stCurrentMeasurement.dDistanceMeters);
            globals::g_pStateMachineHandler->HandleEvent(Event::eMarkerUnseen);
            return;
        }
 
        // Identify target marker.
        tagdetectutils::ArucoTag stBestArucoTag, stBestTorchTag;
        statemachine::IdentifyTargetMarker(m_vTagDetectors, stBestArucoTag, stBestTorchTag, m_stGoalWaypoint.nID);
 
        // Persistent variables to track target across frames
        static double dHeadingSetPoint = 0.0;
        static double dDistanceFromTag = 0.0;
        static geoops::Waypoint stLastGeolocatedPosition;
        static bool bHasLastGeolocatedPosition                     = false;
        static std::chrono::system_clock::time_point tLastSeenTime = std::chrono::system_clock::now();
        static bool bAlreadyPrintedLost                            = false;
        static bool bAlreadyPrintedVisualLostFallback              = false;
 
        // Check for stale session data (if we re-entered this state after a long time).
        if (std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now() - tLastSeenTime).count() >
            constants::APPROACH_MARKER_LOST_GIVE_UP_TIME + 5.0)
        {
            bHasLastGeolocatedPosition = false;
        }
 
        // Check if both tag types are unseen.
        if (stBestArucoTag.nID == -1 && stBestTorchTag.dConfidence == 0.0)
        {
            std::chrono::system_clock::time_point tmCurrentTime = std::chrono::system_clock::now();
            if ((std::chrono::duration_cast<std::chrono::milliseconds>(tmCurrentTime - tLastSeenTime).count() / 1000.0) > constants::APPROACH_MARKER_LOST_GIVE_UP_TIME)
            {
                // Submit logger message.
                globals::g_pStateMachineHandler->HandleEvent(Event::eMarkerUnseen);
                return;
            }
            else
            {
                if (!bAlreadyPrintedLost)
                {
                    bAlreadyPrintedLost = true;
                    // Submit logger message.
                    LOG_WARNING(logging::g_qSharedLogger, "ApproachingMarkerState: No tags detected.");
                }
 
                // If we have a valid last known geolocated position, keep driving to it.
                if (bHasLastGeolocatedPosition)
                {
                    // Calculate the geomeasurement to the last known position.
                    geoops::GeoMeasurement stLastMeasurement =
                        geoops::CalculateGeoMeasurement(stCurrentRoverPose.GetUTMCoordinate(), stLastGeolocatedPosition.GetUTMCoordinate());
 
                    // Update setpoints to track the last known location.
                    dHeadingSetPoint = stLastMeasurement.dStartRelativeBearing;
                    dDistanceFromTag = stLastMeasurement.dDistanceMeters;
 
                    if (!bAlreadyPrintedVisualLostFallback)
                    {
                        LOG_NOTICE(logging::g_qSharedLogger, "ApproachingMarkerState: Visual lost, driving to last known geolocated position.");
                        bAlreadyPrintedVisualLostFallback = true;
                    }
                }
                else
                {
                    // No valid history to fallback on, stop the drive.
                    globals::g_pDriveBoard->SendStop();
                    return;
                }
            }
        }
        else    // Tag Detected
        {
            // Submit logger message.
            if (bAlreadyPrintedLost)
            {
                LOG_NOTICE(logging::g_qSharedLogger, "ApproachingMarkerState: Tags were detected again.");
            }
 
            // Reset the last seen time if a tag is detected.
            tLastSeenTime = std::chrono::system_clock::now();
            // Reset printed flags when tags are detected again
            bAlreadyPrintedLost               = false;
            bAlreadyPrintedVisualLostFallback = false;
 
            // Check if we got a good OpenCV tag.
            if (stBestArucoTag.nID != -1)
            {
                dDistanceFromTag = stBestArucoTag.dStraightLineDistance;
                // Check if the tag has an absolute coordinate populated.
                if (stBestArucoTag.stGeolocatedPosition.eType != geoops::WaypointType::eUNKNOWN)
                {
                    // Calculate the geomeasurement to the tag.
                    geoops::GeoMeasurement stTagMeasurement =
                        geoops::CalculateGeoMeasurement(stCurrentRoverPose.GetUTMCoordinate(), stBestArucoTag.stGeolocatedPosition.GetUTMCoordinate());
                    // Update static variables.
                    dHeadingSetPoint = stTagMeasurement.dStartRelativeBearing;
 
                    // Save geolocated position for fallback.
                    stLastGeolocatedPosition   = stBestArucoTag.stGeolocatedPosition;
                    bHasLastGeolocatedPosition = true;
 
                    // Add the most recent geolocated tag to the plot.
                    m_pRoverPathPlot->AddDot(stBestArucoTag.stGeolocatedPosition.GetUTMCoordinate(), "DetectedTags");
                }
                else
                {
                    // Fallback to relative tracking (Current Heading + Yaw Angle).
                    dHeadingSetPoint = numops::InputAngleModulus(stBestArucoTag.dYawAngle + stCurrentRoverPose.GetCompassHeading(), 0.0, 360.0);
                }
            }
            // Check if we got a good Torch tag.
            else if (stBestTorchTag.dConfidence != 0.0)
            {
                dDistanceFromTag = stBestTorchTag.dStraightLineDistance;
                // Check if the tag has an absolute coordinate populated.
                if (stBestTorchTag.stGeolocatedPosition.eType != geoops::WaypointType::eUNKNOWN)
                {
                    // Calculate the geomeasurement to the tag.
                    geoops::GeoMeasurement stTagMeasurement =
                        geoops::CalculateGeoMeasurement(stCurrentRoverPose.GetUTMCoordinate(), stBestTorchTag.stGeolocatedPosition.GetUTMCoordinate());
                    // Update static variables.
                    dHeadingSetPoint = stTagMeasurement.dStartRelativeBearing;
 
                    // Save geolocated position for fallback.
                    stLastGeolocatedPosition   = stBestTorchTag.stGeolocatedPosition;
                    bHasLastGeolocatedPosition = true;
 
                    // Add the most recent geolocated tag to the plot.
                    m_pRoverPathPlot->AddDot(stBestTorchTag.stGeolocatedPosition.GetUTMCoordinate(), "DetectedTags");
                }
                else
                {
                    // Fallback to relative tracking (Current Heading + Yaw Angle).
                    dHeadingSetPoint = numops::InputAngleModulus(stBestTorchTag.dYawAngle + stCurrentRoverPose.GetCompassHeading(), 0.0, 360.0);
                }
            }
        }
 
        // Move the rover to the target's estimated position.
        diffdrive::DrivePowers stDrivePowers = globals::g_pDriveBoard->CalculateMove(constants::APPROACH_MARKER_MOTOR_POWER,
                                                                                     dHeadingSetPoint,
                                                                                     stCurrentRoverPose.GetCompassHeading(),
                                                                                     diffdrive::DifferentialControlMethod::eArcadeDrive);
        globals::g_pDriveBoard->SendDrive(stDrivePowers);
 
        // Static variable to track last log time.
        static std::chrono::system_clock::time_point tmLastLogTime = std::chrono::system_clock::now();
        std::chrono::system_clock::time_point tmCurrentTime        = std::chrono::system_clock::now();
        // Only log once per second.
        if (std::chrono::duration_cast<std::chrono::seconds>(tmCurrentTime - tmLastLogTime).count() >= 1)
        {
            // Update the last log time.
            tmLastLogTime = tmCurrentTime;
 
            if (stBestArucoTag.nID != -1)
            {
                LOG_NOTICE(logging::g_qSharedLogger,
                           "ApproachingMarkerState: OpenCV Tag ID: {}, Distance: {:.2f} m, Heading: {:.2f} deg",
                           stBestArucoTag.nID,
                           dDistanceFromTag,
                           dHeadingSetPoint);
            }
            else if (stBestTorchTag.dConfidence != 0.0)
            {
                LOG_NOTICE(logging::g_qSharedLogger,
                           "ApproachingMarkerState: Torch Tag Confidence: {:.2f}, Distance: {:.2f} m, Heading: {:.2f} deg",
                           stBestTorchTag.dConfidence,
                           dDistanceFromTag,
                           dHeadingSetPoint);
            }
        }
 
        // Check if tag is reached.
        if (dDistanceFromTag != 0.0 && dDistanceFromTag < constants::APPROACH_MARKER_PROXIMITY_THRESHOLD)
        {
            // Submit logger message.
            LOG_NOTICE(logging::g_qSharedLogger, "ApproachingMarkerState: Rover has reached the target marker!");
            // Check if the OpenCV tag has a good absolute position.
            if (stBestArucoTag.nID != -1 && stBestArucoTag.stGeolocatedPosition.eType == geoops::WaypointType::eTagWaypoint)
            {
                // Add the tag to the path plot.
                m_pRoverPathPlot->AddDot(stBestArucoTag.stGeolocatedPosition.GetUTMCoordinate(), "FinalTag", 7);
            }
            // Check if the torch tag has a good absolute position.
            if (stBestTorchTag.dConfidence != 0.0 && stBestTorchTag.stGeolocatedPosition.eType == geoops::WaypointType::eTagWaypoint)
            {
                // Add the tag to the path plot.
                m_pRoverPathPlot->AddDot(stBestTorchTag.stGeolocatedPosition.GetUTMCoordinate(), "FinalTag", 7);
            }
 
            // Reset the tag heading and distance.
            dHeadingSetPoint           = 0.0;
            dDistanceFromTag           = 0.0;
            bHasLastGeolocatedPosition = false;
 
            // Handle state transition and save the current search pattern state.
            globals::g_pStateMachineHandler->HandleEvent(Event::eReachedMarker, true);
            // Don't execute the rest of the state.
            return;
        }
 
        // ---  Check if the rover is stuck --- //
 
        // Check if stuck.
        if (constants::APPROACH_MARKER_ENABLE_STUCK_DETECT &&
            m_StuckDetector.CheckIfStuck(globals::g_pStateMachineHandler->SmartRetrieveVelocity(), globals::g_pStateMachineHandler->SmartRetrieveAngularVelocity()))
        {
            // Submit logger message.
            LOG_NOTICE(logging::g_qSharedLogger, "ApproachingMarkerState: Rover has become stuck!");
            // Handle state transition and save the current search pattern state.
            globals::g_pStateMachineHandler->HandleEvent(Event::eStuck, true);
            // Don't execute the rest of the state.
            return;
        }
 
        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.");
 
                // Check if verifying marker state is enabled.
                if (constants::APPROACH_MARKER_VERIFY_POSITION)
                {
                    // Change states.
                    eNextState = States::eVerifyingMarker;
                }
                else
                {
                    // 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->ClearSavedStates();
                    // Submit logger message.
                    LOG_NOTICE(logging::g_qSharedLogger, "ApproachingMarkerState: Cleared old search pattern state and approaching marker state from saved states.");
                    // Change state.
                    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::eOff);
                // 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