StateMachineHandler Class Reference

The StateMachineHandler class serves as the main state machine for Autonomy Software. It will handle all state transitions and run the logic for each state. More...

#include <StateMachineHandler.h>

Inheritance diagram for StateMachineHandler:

Collaboration diagram for StateMachineHandler:

Public Member Functions
	StateMachineHandler ()
	Construct a new State Machine Handler object.
	~StateMachineHandler ()
	Destroy the State Machine Handler:: State Machine Handler object.
void	StartStateMachine ()
	This method will start the state machine. It will set the first state to Idle and start the thread pool.
void	StopStateMachine ()
	This method will stop the state machine. It will signal whatever state is currently running to abort back to idle and then stop the main code running in the ThreadedContinuousCode() method.
void	HandleEvent (statemachine::Event eEvent, const bool bSaveCurrentState=false)
	This method Handles Events that are passed to the State Machine Handler. It will check the current state and run the state's HandleEvent() method. It will then check the state's transition conditions and transition to the next state if the conditions are met.
void	ClearSavedStates ()
	Clear all saved states.
void	ClearSavedState (statemachine::States eState)
	Clear a saved state based on the given state.
statemachine::States	GetCurrentState () const
	Accessor for the Current State private member.
statemachine::States	GetPreviousState () const
	Accessor for the Previous State private member.
geoops::RoverPose	SmartRetrieveRoverPose (bool bVIOHeading=true, bool bVIOTracking=false)
	Retrieve the rover's current position and heading. Automatically picks between getting the position/heading from the NavBoard, ZEDSDK Fusion module, or ZED positional tracking. In most cases, this will be the method that should be called over getting the data directly from NavBoard.
double	SmartRetrieveVelocity ()
	Retrieve the rover's current velocity. Currently there is no easy way to get the velocity of the ZEDCam so this method just returns the GPS-based velocity.
double	SmartRetrieveAngularVelocity ()
	Retrieve the rover's current velocity. Currently there is no easy way to get the velocity of the ZEDCam so this method just returns the GPS-based velocity.
void	RealignZEDPosition (CameraHandler::ZEDCamName eCameraName, const geoops::UTMCoordinate &stNewCameraPosition, const double dNewCameraHeading)
	This is used to realign the ZEDs forward direction with the rover's current compass heading. Realigning GPS latlon is not necessary since we're using the ZEDSDK's Fusion module. The heading of the camera's GeoPose is actually automatically realigned too depending on what direction We are headed, but this is just to be safe.
IPS &	GetIPS ()
	Accessor for the Frame I P S private member.

Private Member Functions
std::shared_ptr< statemachine::State >	CreateState (statemachine::States eState)
	Create a State object based of of the State enum.
void	ChangeState (statemachine::States eNextState, const bool bSaveCurrentState=false)
	Transition to a new state. This function is called by the HandleEvent and checks to see if this state is already stored in the map of exited states. If it is, it loads the state from the map. If not, it creates a new state and stores it in the map.
void	SaveCurrentState ()
	Save the current state to the map of exited states. This is used to store the state when the state machine is transitioning to a new state. And prevents the state from being deleted when the state machine transitions to a new state.
void	ThreadedContinuousCode () override
	This code will run continuously in a separate thread. The State Machine Handler will check the current state and run the state's logic. It will then check the state's transition conditions and transition to the next state if the conditions are met.
void	PooledLinearCode () override
	This method holds the code that is ran in the thread pool started by the ThreadedLinearCode() method. It currently does nothing and is not needed in the current implementation of the StateMachineHandler.
Private Member Functions inherited from AutonomyThread< void >
	AutonomyThread ()
	Construct a new Autonomy Thread object.
virtual	~AutonomyThread ()
	Destroy the Autonomy Thread object. If the parent object or main thread is destroyed or exited while this thread is still running, a race condition will occur. Stopping and joining the thread here insures that the main program can't exit if the user forgot to stop and join the thread.
void	Start ()
	When this method is called, it starts a new thread that runs the code within the ThreadedContinuousCode method. This is the users main code that will run the important and continuous code for the class.
void	RequestStop ()
	Signals threads to stop executing user code, terminate. DOES NOT JOIN. This method will not force the thread to exit, if the user code is not written properly and contains WHILE statement or any other long-executing or blocking code, then the thread will not exit until the next iteration.
void	Join ()
	Waits for thread to finish executing and then closes thread. This method will block the calling code until thread is finished.
bool	Joinable () const
	Check if the code within the thread and all pools created by it are finished executing and the thread is ready to be closed.
AutonomyThreadState	GetThreadState () const
	Accessor for the Threads State private member.
IPS &	GetIPS ()
	Accessor for the Frame I P S private member.
void	RunPool (const unsigned int nNumTasksToQueue, const unsigned int nNumThreads=2, const bool bForceStopCurrentThreads=false)
	When this method is called, it starts/adds tasks to a thread pool that runs nNumTasksToQueue copies of the code within the PooledLinearCode() method using nNumThreads number of threads. This is meant to be used as an internal utility of the child class to further improve parallelization. Default value for nNumThreads is 2.
void	RunDetachedPool (const unsigned int nNumTasksToQueue, const unsigned int nNumThreads=2, const bool bForceStopCurrentThreads=false)
	When this method is called, it starts a thread pool full of threads that don't return std::futures (like a placeholder for the thread return type). This means the thread will not have a return type and there is no way to determine if the thread has finished other than calling the Join() method. Only use this if you want to 'set and forget'. It will be faster as it doesn't return futures. Runs PooledLinearCode() method code. This is meant to be used as an internal utility of the child class to further improve parallelization.
void	ParallelizeLoop (const int nNumThreads, const N tTotalIterations, F &&tLoopFunction)
	Given a ref-qualified looping function and an arbitrary number of iterations, this method will divide up the loop and run each section in a thread pool. This function must not return anything. This method will block until the loop has completed.
void	ClearPoolQueue ()
	Clears any tasks waiting to be ran in the queue, tasks currently running will remain running.
void	JoinPool ()
	Waits for pool to finish executing tasks. This method will block the calling code until thread is finished.
bool	PoolJoinable () const
	Check if the internal pool threads are done executing code and the queue is empty.
void	SetMainThreadIPSLimit (int nMaxIterationsPerSecond=0)
	Mutator for the Main Thread Max I P S private member.
int	GetPoolNumOfThreads ()
	Accessor for the Pool Num Of Threads private member.
int	GetPoolQueueLength ()
	Accessor for the Pool Queue Size private member.
std::vector< void >	GetPoolResults ()
	Accessor for the Pool Results private member. The action of getting results will destroy and remove them from this object. This method blocks if the thread is not finished, so no need to call JoinPool() before getting results.
int	GetMainThreadMaxIPS () const
	Accessor for the Main Thread Max I P S private member.

Private Attributes
std::shared_ptr< statemachine::State >	m_pCurrentState
std::shared_ptr< statemachine::State >	m_pPreviousState
std::unordered_map< statemachine::States, std::shared_ptr< statemachine::State > >	m_umSavedStates
std::shared_mutex	m_muStateMutex
std::shared_mutex	m_muEventMutex
std::atomic_bool	m_bSwitchingStates
std::shared_ptr< ZEDCamera >	m_pMainCam
geoops::GPSCoordinate	m_stCurrentGPSLocation
const std::function< void(const rovecomm::RoveCommPacket< uint8_t > &, const sockaddr_in &)>	AutonomyStartCallback
	Callback function used to trigger the start of autonomy. No matter what state we are in, signal a StartAutonomy Event.
const std::function< void(const rovecomm::RoveCommPacket< uint8_t > &, const sockaddr_in &)>	AutonomyStopCallback
	Callback function used to trigger autonomy to stop. No matter what state we are in, signal an Abort Event.
const std::function< void(const rovecomm::RoveCommPacket< uint8_t > &, const sockaddr_in &)>	ClearWaypointsCallback
	Callback function that is called whenever RoveComm receives new CLEARWAYPOINTS packet.
const std::function< void(const rovecomm::RoveCommPacket< float > &, const sockaddr_in &)>	PMSCellVoltageCallback
	Callback function used to force autonomy into Idle state if battery voltage gets too low. No matter what state we are in, signal an Abort Event.
Private Attributes inherited from AutonomyThread< void >
IPS	m_IPS

Additional Inherited Members
Private Types inherited from AutonomyThread< void >
enum	AutonomyThreadState

Detailed Description

The StateMachineHandler class serves as the main state machine for Autonomy Software. It will handle all state transitions and run the logic for each state.

Author

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

Date

2024-01-17

Constructor & Destructor Documentation

StateMachineHandler()

StateMachineHandler::StateMachineHandler

(

)

Construct a new State Machine Handler object.

Author

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

Date

2024-01-17

{
    // Submit logger message.
    LOG_INFO(logging::g_qSharedLogger, "Initializing State Machine.");
 
    // Subscribe to PMS packets.
    rovecomm::RoveCommPacket<u_int8_t> stSubscribePacket;
    stSubscribePacket.unDataId    = manifest::System::SUBSCRIBE_DATA_ID;
    stSubscribePacket.unDataCount = 0;
    stSubscribePacket.eDataType   = manifest::DataTypes::UINT8_T;
    stSubscribePacket.vData       = std::vector<uint8_t>{};
    network::g_pRoveCommUDPNode->SendUDPPacket(stSubscribePacket, manifest::PMS::IP_ADDRESS.IP_STR.c_str(), constants::ROVECOMM_OUTGOING_UDP_PORT);
 
    // Set RoveComm Node callbacks.
    network::g_pRoveCommUDPNode->AddUDPCallback<uint8_t>(AutonomyStartCallback, manifest::Autonomy::COMMANDS.find("STARTAUTONOMY")->second.DATA_ID);
    network::g_pRoveCommUDPNode->AddUDPCallback<uint8_t>(AutonomyStopCallback, manifest::Autonomy::COMMANDS.find("DISABLEAUTONOMY")->second.DATA_ID);
    network::g_pRoveCommUDPNode->AddUDPCallback<uint8_t>(ClearWaypointsCallback, manifest::Autonomy::COMMANDS.find("CLEARWAYPOINTS")->second.DATA_ID);
    network::g_pRoveCommUDPNode->AddUDPCallback<float>(PMSCellVoltageCallback, manifest::PMS::TELEMETRY.find("CELLVOLTAGE")->second.DATA_ID);
 
    // Initialize member variables.
    m_pMainCam = globals::g_pCameraHandler->GetZED(CameraHandler::ZEDCamName::eHeadMainCam);
 
    // State machine doesn't need to run at an unlimited speed. Cap main thread to a certain amount of iterations per second.
    this->SetMainThreadIPSLimit(constants::STATEMACHINE_MAX_IPS);
}

Here is the call graph for this function:

~StateMachineHandler()

StateMachineHandler::~StateMachineHandler

(

)

Destroy the State Machine Handler:: State Machine Handler object.

Author

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

Date

2024-03-15

{
    // Check if state machine is running.
    if (this->GetThreadState() == AutonomyThreadState::eRunning)
    {
        // Stop state machine.
        this->StopStateMachine();
    }
}

Here is the call graph for this function:

Member Function Documentation

CreateState()

std::shared_ptr< statemachine::State > StateMachineHandler::CreateState ( statemachine::States  eState )

private

Create a State object based of of the State enum.

Parameters

eState

- The State enum to create a State object from.

Returns

std::shared_ptr<State> - The State object created from the State enum.

Author

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

Date

2024-01-17

{
    switch (eState)
    {
        case statemachine::States::eIdle: return std::make_shared<statemachine::IdleState>();
        case statemachine::States::eNavigating: return std::make_shared<statemachine::NavigatingState>();
        case statemachine::States::eSearchPattern: return std::make_shared<statemachine::SearchPatternState>();
        case statemachine::States::eApproachingMarker: return std::make_shared<statemachine::ApproachingMarkerState>();
        case statemachine::States::eApproachingObject: return std::make_shared<statemachine::ApproachingObjectState>();
        case statemachine::States::eVerifyingPosition: return std::make_shared<statemachine::VerifyingPositionState>();
        case statemachine::States::eVerifyingMarker: return std::make_shared<statemachine::VerifyingMarkerState>();
        case statemachine::States::eVerifyingObject: return std::make_shared<statemachine::VerifyingObjectState>();
        case statemachine::States::eAvoidance: return std::make_shared<statemachine::AvoidanceState>();
        case statemachine::States::eReversing: return std::make_shared<statemachine::ReversingState>();
        case statemachine::States::eStuck: return std::make_shared<statemachine::StuckState>();
        default:
            // Handle the default case or throw an exception if needed
            LOG_ERROR(logging::g_qSharedLogger, "State {} not found.", static_cast<int>(eState));
    }
 
    return nullptr;
}

Here is the caller graph for this function:

ChangeState()

void StateMachineHandler::ChangeState ( statemachine::States  eNextState, const bool  bSaveCurrentState = false  )

private

Transition to a new state. This function is called by the HandleEvent and checks to see if this state is already stored in the map of exited states. If it is, it loads the state from the map. If not, it creates a new state and stores it in the map.

Parameters

eNextState	- The State enum to transition to.
bSaveCurrentState	- Whether or not to save the current state so it can be recalled next time it is triggered. Default value is false.

Author

Eli Byrd (edbgk.nosp@m.k@ms.nosp@m.t.edu), clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-01-17

{
    // Acquire write lock for changing states.
    std::unique_lock<std::shared_mutex> lkStateProcessLock(m_muStateMutex);
 
    // Check if we are already in this state.
    if (m_pCurrentState->GetState() != eNextState)
    {
        // Set atomic toggle saying we are in the process if switching states.
        m_bSwitchingStates = true;
 
        // Save the current state as the previous state
        m_pPreviousState = m_pCurrentState;
 
        // Check if we should save this current state so it can be recalled in the future.
        if (bSaveCurrentState)
        {
            // Save the current state before transitioning
            SaveCurrentState();
        }
 
        // Check if the state exists in exitedStates
        std::unordered_map<statemachine::States, std::shared_ptr<statemachine::State>>::iterator itState = m_umSavedStates.find(eNextState);
        if (itState != m_umSavedStates.end())
        {
            // Load the existing state
            m_pCurrentState = itState->second;
            // Remove new current state state from saved states.
            m_umSavedStates.erase(eNextState);
 
            // Submit logger message.
            LOG_INFO(logging::g_qSharedLogger, "Recalling State: {}", m_pCurrentState->ToString());
        }
        else
        {
            // Create and enter a new state
            m_pCurrentState = CreateState(eNextState);
        }
 
        // Set atomic toggle saying we are done switching states.
        m_bSwitchingStates = false;
    }
 
    // Send current robot state over RoveComm.
    rovecomm::RoveCommPacket<uint8_t> stPacket;
    stPacket.unDataId    = manifest::Autonomy::TELEMETRY.find("CURRENTSTATE")->second.DATA_ID;
    stPacket.unDataCount = manifest::Autonomy::TELEMETRY.find("CURRENTSTATE")->second.DATA_COUNT;
    stPacket.eDataType   = manifest::Autonomy::TELEMETRY.find("CURRENTSTATE")->second.DATA_TYPE;
    stPacket.vData.emplace_back(static_cast<uint8_t>(this->GetCurrentState()));
    // Send drive command over RoveComm to drive board to all subscribers.
    network::g_pRoveCommUDPNode->SendUDPPacket(stPacket, "0.0.0.0", constants::ROVECOMM_OUTGOING_UDP_PORT);
}

Here is the call graph for this function:

Here is the caller graph for this function:

SaveCurrentState()

void StateMachineHandler::SaveCurrentState ( )

private

Save the current state to the map of exited states. This is used to store the state when the state machine is transitioning to a new state. And prevents the state from being deleted when the state machine transitions to a new state.

Author

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

Date

2024-01-17

{
    // Submit logger message.
    LOG_INFO(logging::g_qSharedLogger, "Saving State: {}", m_pCurrentState->ToString());
    // Add state to map.
    m_umSavedStates[m_pCurrentState->GetState()] = m_pCurrentState;
}

Here is the caller graph for this function:

ThreadedContinuousCode()

void StateMachineHandler::ThreadedContinuousCode ( )

overrideprivatevirtual

This code will run continuously in a separate thread. The State Machine Handler will check the current state and run the state's logic. It will then check the state's transition conditions and transition to the next state if the conditions are met.

Author

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

Date

2024-01-17

Implements AutonomyThread< void >.

{
    /*
        Verify that the state machine has been initialized so that it doesn't
        try to run before a state has been initialized. Also verify that the
        state machine is not currently switching states. This prevents the
        state machine from running while it is in the middle of switching
        states. And verify that the state machine is not exiting. This prevents
        the state machine from running after it has been stopped.
    */
    if (!m_bSwitchingStates)
    {
        // Run the current state
        m_pCurrentState->Run();
    }
 
    // Create instance variable.
    static geoops::GPSCoordinate stNewGPSLocation;
    // Check if GPS data is recent and updated.
    if (std::chrono::duration_cast<std::chrono::milliseconds>(globals::g_pNavigationBoard->GetGPSLastUpdateTime()).count() <= 100 ||
        (stNewGPSLocation.dLatitude == 0.0 && stNewGPSLocation.dLongitude == 0.0))
    {
        // Get the current NavBoard GPS data.
        stNewGPSLocation = globals::g_pNavigationBoard->GetGPSData();
    }
 
    // Create static boolean value for toggling DiffGPS warning log print.
    static bool bAlreadyPrintedDiffGPSWarning = false;
    // Check if the current GPS data is different from the old.
    if (stNewGPSLocation.dLatitude != m_stCurrentGPSLocation.dLatitude && stNewGPSLocation.dLongitude != m_stCurrentGPSLocation.dLongitude &&
        stNewGPSLocation.dAltitude != m_stCurrentGPSLocation.dAltitude)
    {
        // Check if GNSS fusion is enabled.
        if (constants::FUSION_ENABLE_GNSS_FUSION)
        {
            // Check if our GPS coordinate from NavBoard is differential.
            if (stNewGPSLocation.bIsDifferential)
            {
                // Check if main ZED camera is setup to use GPS fusion.
                if (m_pMainCam->GetIsFusionMaster() && m_pMainCam->GetPositionalTrackingEnabled())
                {
                    // Update current GPS position.
                    m_stCurrentGPSLocation = stNewGPSLocation;
                    // Feed current GPS location to main ZED camera.
                    m_pMainCam->IngestGPSDataToFusion(m_stCurrentGPSLocation);
                }
 
                // Reset DiffGPS warning print toggle.
                if (bAlreadyPrintedDiffGPSWarning)
                {
                    // Submit logger message.
                    LOG_NOTICE(logging::g_qSharedLogger,
                               "Incoming GPS position to NavBoard now has differential accuracy! Autonomy will switch to using GPS Fusion for high accuracy navigation!");
 
                    // Rest toggle.
                    bAlreadyPrintedDiffGPSWarning = false;
                }
            }
            // Check if GPS coordinate from NavBoard is not differential and print warning log.
            else if (!bAlreadyPrintedDiffGPSWarning)
            {
                // Submit logger message.
                LOG_NOTICE(logging::g_qSharedLogger,
                           "Incoming GPS position to NavBoard does not have differential accuracy! Autonomy will not use GPS Fusion but instead fallback to aligning "
                           "the ZED pose while the rover is in Idle state and not moving. Autonomous navigation performance of the rover will be degraded...");
 
                // Set already printed toggle.
                bAlreadyPrintedDiffGPSWarning = true;
            }
        }
 
        // Realign the camera's relative position to current GPS position when in Idle. This does not affect fusion, but makes sure we can fallback to the camera pose for
        // positioning.
        if (m_pCurrentState->GetState() == statemachine::States::eIdle && m_pMainCam->GetPositionalTrackingEnabled())
        {
            // Check if the rover is currently not driving of turning. Use only GPS based and use stuck state parameters for checking.
            if (globals::g_pNavigationBoard->GetVelocity() <= constants::STUCK_CHECK_VEL_THRESH &&
                globals::g_pNavigationBoard->GetAngularVelocity() <= constants::STUCK_CHECK_ROT_THRESH)
            {
                // Update current GPS position.
                m_stCurrentGPSLocation = stNewGPSLocation;
                // Get current compass heading.
                double dCurrentCompassHeading = globals::g_pNavigationBoard->GetHeading();
                // Realign the main ZED cameras pose with current GPS-based position and heading.
                this->RealignZEDPosition(CameraHandler::ZEDCamName::eHeadMainCam, geoops::ConvertGPSToUTM(m_stCurrentGPSLocation), dCurrentCompassHeading);
            }
        }
    }
}

Here is the call graph for this function:

PooledLinearCode()

void StateMachineHandler::PooledLinearCode ( )

overrideprivatevirtual

This method holds the code that is ran in the thread pool started by the ThreadedLinearCode() method. It currently does nothing and is not needed in the current implementation of the StateMachineHandler.

Author

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

Date

2024-01-17

Implements AutonomyThread< void >.

{}

StartStateMachine()

void StateMachineHandler::StartStateMachine

(

)

This method will start the state machine. It will set the first state to Idle and start the thread pool.

Author

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

Date

2024-01-17

{
    // Initialize the state machine with the initial state
    m_pCurrentState    = CreateState(statemachine::States::eIdle);
    m_bSwitchingStates = false;
 
    // Clear any saved states.
    this->ClearSavedStates();
 
    // Start the state machine thread
    Start();
 
    // Submit logger message.
    LOG_INFO(logging::g_qSharedLogger, "Started State Machine.");
}

Here is the call graph for this function:

Here is the caller graph for this function:

StopStateMachine()

void StateMachineHandler::StopStateMachine

(

)

This method will stop the state machine. It will signal whatever state is currently running to abort back to idle and then stop the main code running in the ThreadedContinuousCode() method.

Author

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

Date

2024-01-19

{
    // No matter the current state, abort back to idle.
    this->HandleEvent(statemachine::Event::eAbort);
 
    // Stop main thread.
    this->RequestStop();
    this->Join();
 
    // Send multimedia command to update state display.
    globals::g_pMultimediaBoard->SendLightingState(MultimediaBoard::MultimediaBoardLightingState::eOff);
    // Stop drive.
    globals::g_pDriveBoard->SendStop();
 
    // Submit logger message.
    LOG_INFO(logging::g_qSharedLogger, "Stopped State Machine.");
}

Here is the call graph for this function:

Here is the caller graph for this function:

HandleEvent()

void StateMachineHandler::HandleEvent	(	statemachine::Event	eEvent,
		const bool	bSaveCurrentState = false
	)

This method Handles Events that are passed to the State Machine Handler. It will check the current state and run the state's HandleEvent() method. It will then check the state's transition conditions and transition to the next state if the conditions are met.

Parameters

eEvent	- The Event enum to handle.
bSaveCurrentState	- Whether or not to save the current state so it can be recalled next time it is triggered. Default value is false.

Author

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

Date

2024-01-17

{
    // Acquire write lock for handling events.
    std::unique_lock<std::shared_mutex> lkEventProcessLock(m_muEventMutex);
 
    // Stop the drive.
    globals::g_pDriveBoard->SendStop();
 
    // Check if the current state is not null and the state machine is running.
    if (m_pCurrentState != nullptr && this->GetThreadState() == AutonomyThreadState::eRunning)
    {
        // Trigger the event on the current state
        statemachine::States eNextState = m_pCurrentState->TriggerEvent(eEvent);
 
        // Transition to the next state
        ChangeState(eNextState, bSaveCurrentState);
    }
}

Here is the call graph for this function:

Here is the caller graph for this function:

ClearSavedStates()

void StateMachineHandler::ClearSavedStates

(

)

Clear all saved states.

Author

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

Date

2024-04-01

{
    // Acquire write lock for clearing saved states.
    std::unique_lock<std::shared_mutex> lkStateProcessLock(m_muStateMutex);
    // Clear all saved states.
    m_umSavedStates.clear();
    // Reset previous state to nullptr;
    m_pPreviousState = std::make_shared<statemachine::IdleState>();
}

Here is the caller graph for this function:

ClearSavedState()

void StateMachineHandler::ClearSavedState

(

statemachine::States

eState

)

Clear a saved state based on the given state.

Parameters

eState

- The state to clear from the saved states.

Author

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

Date

2025-01-06

{
    // Acquire write lock for clearing saved states.
    std::unique_lock<std::shared_mutex> lkStateProcessLock(m_muStateMutex);
    // Remove all states that match the given state.
    m_umSavedStates.erase(eState);
}

GetCurrentState()

statemachine::States StateMachineHandler::GetCurrentState

(

)

const

Accessor for the Current State private member.

Returns

States - The current state of the state machine.

Author

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

Date

2024-01-17

{
    return m_pCurrentState->GetState();
}

Here is the caller graph for this function:

GetPreviousState()

statemachine::States StateMachineHandler::GetPreviousState

(

)

const

Accessor for the Previous State private member.

Returns

States - The previous state of the state machine.

Author

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

Date

2024-01-17

{
    // Check if the previous state exists and return it if it does otherwise return Idle
    return m_pPreviousState ? m_pPreviousState->GetState() : statemachine::States::eIdle;
}

Here is the caller graph for this function:

SmartRetrieveRoverPose()

geoops::RoverPose StateMachineHandler::SmartRetrieveRoverPose	(	bool	bVIOHeading = true,
		bool	bVIOTracking = false
	)

Retrieve the rover's current position and heading. Automatically picks between getting the position/heading from the NavBoard, ZEDSDK Fusion module, or ZED positional tracking. In most cases, this will be the method that should be called over getting the data directly from NavBoard.

Parameters

bVIOHeading	- Whether to use ZED Heading Fusion.
bVIOTracking	- Whether to use ZED Positional Tracking.

Returns

geoops::RoverPose - The current position and heading (pose) of the rover stored in a RoverPose struct.

Author

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

Date

2024-04-06

{
    // Get and store the normal GPS position and heading from NavBoard.
    geoops::GPSCoordinate stCurrentGPSPosition = globals::g_pNavigationBoard->GetGPSData();
    double dCurrentGPSHeading                  = globals::g_pNavigationBoard->GetHeading();
 
    // Create instance variables.
    std::shared_ptr<ZEDCamera> pMainCam        = globals::g_pCameraHandler->GetZED(CameraHandler::ZEDCamName::eHeadMainCam);
    geoops::GPSCoordinate stCurrentVIOPosition = stCurrentGPSPosition;
    double dCurrentHeading                     = dCurrentGPSHeading;
    bool bVIOGPSFused                          = false;
    static bool bAlreadyPrinted                = false;
 
    if ((bVIOHeading || bVIOTracking) && !constants::MODE_SIM)
    {
        // Check if the main ZED camera is opened and the fusion module is initialized.
        if (pMainCam->GetCameraIsOpen() && pMainCam->GetPositionalTrackingEnabled())
        {
            // Get the fused positional tracking state of the main camera.
            sl::GNSS_FUSION_STATUS slGNSSFusionStatus = pMainCam->GetFusedPositionalTrackingState().gnss_fusion_status;
 
            // Check if GNSS fusion is enabled and current GPS data from has differential accuracy and GNSS fusion is calibrated.
            if (constants::FUSION_ENABLE_GNSS_FUSION && pMainCam->GetIsFusionMaster() && stCurrentGPSPosition.bIsDifferential &&
                (slGNSSFusionStatus == sl::GNSS_FUSION_STATUS::OK || slGNSSFusionStatus == sl::GNSS_FUSION_STATUS::RECALIBRATION_IN_PROGRESS))
            {
                // Create instance variables.
                sl::GeoPose slCurrentCameraGeoPose;
                ZEDCam::Pose stCurrentCameraVIOPose;
 
                // Check if position VIO tracking should be used.
                if (bVIOTracking)
                {
                    // Get the current camera pose from the ZEDCam.
                    std::future<bool> fuResultStatus = pMainCam->RequestFusionGeoPoseCopy(slCurrentCameraGeoPose);
                    if (fuResultStatus.get())
                    {
                        // Repack the camera pose into a GPSCoordinate.
                        stCurrentVIOPosition.dLatitude  = slCurrentCameraGeoPose.latlng_coordinates.getLatitude(false);
                        stCurrentVIOPosition.dLongitude = slCurrentCameraGeoPose.latlng_coordinates.getLongitude(false);
                        stCurrentVIOPosition.dAltitude  = slCurrentCameraGeoPose.latlng_coordinates.getAltitude();
 
                        // Set fused toggle.
                        bVIOGPSFused = true;
                    }
                }
 
                // Check if heading VIO tracking should be used.
                if (bVIOHeading)
                {
                    // Get the current camera pose from the ZEDCam.
                    std::future<bool> fuResultStatus2 = pMainCam->RequestPositionalPoseCopy(stCurrentCameraVIOPose);
                    if (fuResultStatus2.get())
                    {
                        // Repack the camera pose into a UTMCoordinate.
                        dCurrentHeading = stCurrentCameraVIOPose.stEulerAngles.dYO;
 
                        // Set fused toggle.
                        bVIOGPSFused = true;
                    }
                }
 
                // Check toggle so we only print once.
                if (bAlreadyPrinted)
                {
                    // Submit logger message.
                    LOG_NOTICE(logging::g_qSharedLogger, "GNSS Fusion has now converged! Using GNSS Fusion for rover pose...");
                    // Set toggle.
                    bAlreadyPrinted = false;
                }
            }
            else
            {
                // Create instance variables.
                ZEDCam::Pose stCurrentCameraVIOPose;
 
                // Get the current camera pose from the ZEDCam.
                std::future<bool> fuResultStatus = pMainCam->RequestPositionalPoseCopy(stCurrentCameraVIOPose);
                // Wait for future to be fulfilled.
                if (fuResultStatus.get())
                {
                    // Check if position VIO tracking should be used.
                    if (bVIOTracking)
                    {
                        // Camera is using UTM. Modify current GPS position to be camera's position.
                        geoops::UTMCoordinate stCameraUTMLocation = geoops::ConvertGPSToUTM(stCurrentGPSPosition);
                        // Repack the camera pose into a GPSCoordinate.
                        stCameraUTMLocation.dEasting  = stCurrentCameraVIOPose.stTranslation.dX;
                        stCameraUTMLocation.dNorthing = stCurrentCameraVIOPose.stTranslation.dZ;
                        stCameraUTMLocation.dAltitude = stCurrentCameraVIOPose.stTranslation.dY;
                        // Convert back to GPS coordinate and store.
                        stCurrentVIOPosition = geoops::ConvertUTMToGPS(stCameraUTMLocation);
                    }
 
                    // Check if heading VIO tracking should be used.
                    if (bVIOHeading)
                    {
                        // Get compass heading based off of the ZED's aligned accelerometer.
                        dCurrentHeading = stCurrentCameraVIOPose.stEulerAngles.dYO;
                    }
 
                    // Set fused toggle.
                    bVIOGPSFused = false;
                }
 
                // Check toggle so we only print once.
                if (!bAlreadyPrinted && constants::FUSION_ENABLE_GNSS_FUSION)
                {
                    // Submit logger message.
                    LOG_NOTICE(logging::g_qSharedLogger, "GNSS Fusion is still calibrating. Using VIO tracking for rover pose...");
                    // Set toggle.
                    bAlreadyPrinted = true;
                }
            }
        }
        else
        {
            LOG_WARNING_LIMIT(std::chrono::seconds(5),
                              logging::g_qSharedLogger,
                              "Positional tracking is not enabled or camera is not open! Using NavBoard GPS data for rover pose...");
        }
    }
 
    // Submit a debug print for the current rover pose.
    geoops::UTMCoordinate stCurrentUTMPosition = geoops::ConvertGPSToUTM(stCurrentVIOPosition);
    LOG_DEBUG(logging::g_qSharedLogger,
              "Rover Pose is currently: {} (easting), {} (northing), {} (alt), {} (degrees), GNSS/VIO FUSED? = {}, VIOPosition = {}, VIOHeading = {}",
              stCurrentUTMPosition.dEasting,
              stCurrentUTMPosition.dNorthing,
              stCurrentUTMPosition.dAltitude,
              dCurrentHeading,
              bVIOGPSFused ? "true" : "false",
              bVIOTracking ? "true" : "false",
              bVIOHeading ? "true" : "false");
 
    // Check if VIO tracking or heading is being used.
    if (bVIOHeading || bVIOTracking)
    {
        // Submit a debug print for some error metrics pertaining to the ZED camera and NavBoard locations and headings.
        double dHeadingError  = dCurrentHeading - dCurrentGPSHeading;
        double dEastingError  = ConvertGPSToUTM(stCurrentGPSPosition).dEasting - stCurrentUTMPosition.dEasting;
        double dNorthingError = ConvertGPSToUTM(stCurrentGPSPosition).dNorthing - stCurrentUTMPosition.dNorthing;
        // Assemble the error metrics into a single string. We are going to include the original GPS positions of the NavBoard and the Camera and then include the error.
        // Same thing for the heading data.
        std::string szErrorMetrics = "--------[ Pose Tracking Error ]--------\nGPS/VIO Position Error (UTM for easy reading):\n" +
                                     std::to_string(ConvertGPSToUTM(stCurrentGPSPosition).dEasting) + " (NavBoard) vs. " + std::to_string(stCurrentUTMPosition.dEasting) +
                                     " (Camera) = " + std::to_string(dEastingError) + " (error)\n" + std::to_string(ConvertGPSToUTM(stCurrentGPSPosition).dNorthing) +
                                     " (NavBoard) vs. " + std::to_string(stCurrentUTMPosition.dNorthing) + " (Camera) = " + std::to_string(dNorthingError) +
                                     " (error)\n" + "Heading Error:\n" + std::to_string(dCurrentGPSHeading) + " (NavBoard) vs. " + std::to_string(dCurrentHeading) +
                                     " (Camera) = " + std::to_string(dHeadingError) + " (error)\n GNSS/VIO FUSED? = " + (bVIOGPSFused ? "true" : "false") +
                                     ", VIOPosition = " + (bVIOTracking ? "true" : "false") + ", VIOHeading = " + (bVIOHeading ? "true" : "false");
        // Submit the error metrics to the logger.
        LOG_DEBUG(logging::g_qSharedLogger, "{}", szErrorMetrics);
    }
 
    return geoops::RoverPose(stCurrentVIOPosition, dCurrentHeading);
}

Here is the call graph for this function:

Here is the caller graph for this function:

SmartRetrieveVelocity()

double StateMachineHandler::SmartRetrieveVelocity

(

)

Retrieve the rover's current velocity. Currently there is no easy way to get the velocity of the ZEDCam so this method just returns the GPS-based velocity.

Returns

double - The current velocity of the rover. (m/s)

Author

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

Date

2024-04-06

{
    // Return the GPS-based velocity from the NavBoard.
    return globals::g_pNavigationBoard->GetVelocity();
}

Here is the call graph for this function:

Here is the caller graph for this function:

SmartRetrieveAngularVelocity()

double StateMachineHandler::SmartRetrieveAngularVelocity

(

)

Retrieve the rover's current velocity. Currently there is no easy way to get the velocity of the ZEDCam so this method just returns the GPS-based velocity.

Returns

double - The current angular velocity of the rover. (deg/s)

Author

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

Date

2024-04-06

{
    // Return the GPS-based angular velocity from the NavBoard.
    return globals::g_pNavigationBoard->GetAngularVelocity();
}

Here is the call graph for this function:

Here is the caller graph for this function:

RealignZEDPosition()

void StateMachineHandler::RealignZEDPosition	(	CameraHandler::ZEDCamName	eCameraName,
		const geoops::UTMCoordinate &	stNewCameraPosition,
		const double	dNewCameraHeading
	)

This is used to realign the ZEDs forward direction with the rover's current compass heading. Realigning GPS latlon is not necessary since we're using the ZEDSDK's Fusion module. The heading of the camera's GeoPose is actually automatically realigned too depending on what direction We are headed, but this is just to be safe.

Parameters

eCameraName	- The camera name represented as an enum from the CameraHandler class.
stNewCameraPosition	- The new UTM position of the ZED camera.
dNewCameraHeading	- The new compass heading of the ZED camera.

Author

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

Date

2024-04-17

{
    // Get main ZEDCam.
    std::shared_ptr<ZEDCamera> pMainCam = globals::g_pCameraHandler->GetZED(eCameraName);
 
    // Check if main ZEDCam is opened and positional tracking is enabled.
    if (pMainCam->GetCameraIsOpen() && pMainCam->GetPositionalTrackingEnabled())
    {
        // Request for the cameras current pose.
        ZEDCam::Pose stCurrentCameraPose;
        std::future<bool> fuPoseReturnStatus = pMainCam->RequestPositionalPoseCopy(stCurrentCameraPose);
        // Wait for pose to be copied.
        if (fuPoseReturnStatus.get())
        {
            // Pack the current camera pose into UTM coordinate.
            geoops::UTMCoordinate stCurrentCameraUTM = geoops::UTMCoordinate(stCurrentCameraPose.stTranslation.dX,
                                                                             stCurrentCameraPose.stTranslation.dZ,
                                                                             stNewCameraPosition.nZone,
                                                                             stNewCameraPosition.bWithinNorthernHemisphere,
                                                                             stCurrentCameraPose.stTranslation.dY);
            // Calculate the distance between the current and new camera position.
            geoops::GeoMeasurement stError = geoops::CalculateGeoMeasurement(stCurrentCameraUTM, stNewCameraPosition);
 
            // Check if the camera's current pose is close to the new position.
            if (stError.dDistanceMeters >= constants::STATEMACHINE_ZED_REALIGN_THRESHOLD)
            {
                // Update camera Y heading with GPSs current heading.
                pMainCam->SetPositionalPose(stNewCameraPosition.dEasting,
                                            stNewCameraPosition.dAltitude,
                                            stNewCameraPosition.dNorthing,
                                            stCurrentCameraPose.stEulerAngles.dXO,
                                            dNewCameraHeading,
                                            stCurrentCameraPose.stEulerAngles.dZO);
 
                // Submit logger message.
                LOG_INFO(logging::g_qSharedLogger, "Realigned ZED stereo camera to current GPS position. Error was: {} meters.", stError.dDistanceMeters);
            }
        }
        else
        {
            // Submit logger message.
            LOG_ERROR(logging::g_qSharedLogger, "Failed to realign the ZEDCam's pose with the given UTM position and compass heading.");
        }
    }
    else
    {
        // Submit logger message.
        LOG_ERROR(logging::g_qSharedLogger,
                  "Failed to realign the ZEDCam's pose with the given UTM position and compass heading. The camera is not open yet or positional tracking status is "
                  "suboptimal!");
    }
}

Here is the call graph for this function:

Here is the caller graph for this function:

GetIPS()

IPS & AutonomyThread< T >::GetIPS ( )

inline

Accessor for the Frame I P S private member.

Returns

IPS& - The iteration per second counter for the ThreadedContinuousCode()

Author

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

Date

2023-08-20

{ return m_IPS; }

Member Data Documentation

AutonomyStartCallback

const std::function<void(const rovecomm::RoveCommPacket<uint8_t>&, const sockaddr_in&)> StateMachineHandler::AutonomyStartCallback

private

Initial value:

=
            [this](const rovecomm::RoveCommPacket<uint8_t>& stPacket, const sockaddr_in& stdAddr)
        {
            
            (void) stPacket;
            (void) stdAddr;
 
            
            LOG_INFO(logging::g_qSharedLogger, "Incoming Packet: Start Autonomy!");
 
            
            this->HandleEvent(statemachine::Event::eStart);
        }

Callback function used to trigger the start of autonomy. No matter what state we are in, signal a StartAutonomy Event.

Author

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

Date

2024-03-15

        {
            // Not using this.
            (void) stPacket;
            (void) stdAddr;
 
            // Submit logger message.
            LOG_INFO(logging::g_qSharedLogger, "Incoming Packet: Start Autonomy!");
 
            // Signal statemachine handler with Start event.
            this->HandleEvent(statemachine::Event::eStart);
        };

AutonomyStopCallback

const std::function<void(const rovecomm::RoveCommPacket<uint8_t>&, const sockaddr_in&)> StateMachineHandler::AutonomyStopCallback

private

Initial value:

=
            [this](const rovecomm::RoveCommPacket<uint8_t>& stPacket, const sockaddr_in& stdAddr)
        {
            
            (void) stPacket;
            (void) stdAddr;
 
            
            LOG_INFO(logging::g_qSharedLogger, "Incoming Packet: Abort Autonomy!");
 
            
            this->HandleEvent(statemachine::Event::eAbort, true);
        }

Callback function used to trigger autonomy to stop. No matter what state we are in, signal an Abort Event.

Author

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

Date

2024-03-15

        {
            // Not using this.
            (void) stPacket;
            (void) stdAddr;
 
            // Submit logger message.
            LOG_INFO(logging::g_qSharedLogger, "Incoming Packet: Abort Autonomy!");
 
            // Signal statemachine handler with stop event.
            this->HandleEvent(statemachine::Event::eAbort, true);
        };

ClearWaypointsCallback

const std::function<void(const rovecomm::RoveCommPacket<uint8_t>&, const sockaddr_in&)> StateMachineHandler::ClearWaypointsCallback

private

Initial value:

=
            [this](const rovecomm::RoveCommPacket<uint8_t>& stPacket, const sockaddr_in& stdAddr)
        {
            
            (void) stPacket;
            (void) stdAddr;
 
            
 
            
            if (this->GetCurrentState() == statemachine::States::eIdle)
            {
                
                LOG_NOTICE(logging::g_qSharedLogger, "Incoming Clear Waypoints packet: Deleting all saved states in StateMachineHandler...");
                
                this->ClearSavedStates();
            }
            else
            {
                
                LOG_WARNING(logging::g_qSharedLogger,
                            "Incoming Clear Waypoints packet: Cannot clear saved states in StateMachineHandler unless in Idle state. Current state is {}.",
                            static_cast<int>(this->GetCurrentState()));
            }
        }

Callback function that is called whenever RoveComm receives new CLEARWAYPOINTS packet.

Author

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

Date

2024-03-03

        {
            // Not using this.
            (void) stPacket;
            (void) stdAddr;
 
            /*
                The clear waypoints command will clear all waypoints in the WaypointHandler, but it also clears all saved states in the StateMachineHandler
                to prevent any conflicts when restarting autonomy with previously saved states that may have waypoints associated with them.
                However, we only want to delete our saved states if we are currently in IdleState.
            */
 
            // Check if the current state is IdleState.
            if (this->GetCurrentState() == statemachine::States::eIdle)
            {
                // Submit logger message.
                LOG_NOTICE(logging::g_qSharedLogger, "Incoming Clear Waypoints packet: Deleting all saved states in StateMachineHandler...");
                // Clear the saved states.
                this->ClearSavedStates();
            }
            else
            {
                // Submit logger message.
                LOG_WARNING(logging::g_qSharedLogger,
                            "Incoming Clear Waypoints packet: Cannot clear saved states in StateMachineHandler unless in Idle state. Current state is {}.",
                            static_cast<int>(this->GetCurrentState()));
            }
        };

PMSCellVoltageCallback

const std::function<void(const rovecomm::RoveCommPacket<float>&, const sockaddr_in&)> StateMachineHandler::PMSCellVoltageCallback

private

Callback function used to force autonomy into Idle state if battery voltage gets too low. No matter what state we are in, signal an Abort Event.

Author

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

Date

2024-04-04

        {
            // Not using this.
            (void) stdAddr;
 
            // Create instance variables.
            double dTotalCellVoltages   = 0.0;
            int nValidCellVoltageValues = 0;
 
            // Loop through voltage values and average all of the valid ones.
            for (int nIter = 0; nIter < stPacket.unDataCount; ++nIter)
            {
                // Check if the voltage values is greater than at least 0.1.
                if (stPacket.vData[nIter] >= 0.1)
                {
                    // Add cell voltage value to total.
                    dTotalCellVoltages += stPacket.vData[nIter];
                    // Increment voltage voltage counter.
                    ++nValidCellVoltageValues;
                }
            }
            // Calculate average cell voltage.
            double dAverageCellVoltage = dTotalCellVoltages / nValidCellVoltageValues;
 
            // Submit logger message.
            LOG_DEBUG(logging::g_qSharedLogger, "Incoming Packet: PMS Cell Voltages. Average voltage is: {}", dAverageCellVoltage);
 
            // Check if voltage is above the safe minimum for lithium ion batteries.
            if (constants::BATTERY_CHECKS_ENABLED && dAverageCellVoltage < constants::BATTERY_MINIMUM_CELL_VOLTAGE &&
                this->GetCurrentState() != statemachine::States::eIdle)
            {
                // Submit logger message.
                LOG_CRITICAL(logging::g_qSharedLogger,
                             "Incoming PMS Packet: Average cell voltage is {} which is below the safe minimum of {}. Entering Idle state...",
                             dAverageCellVoltage,
                             constants::BATTERY_MINIMUM_CELL_VOLTAGE);
 
                // Signal statemachine handler with stop event.
                this->HandleEvent(statemachine::Event::eAbort, true);
            }
        };

---

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

• src/handlers/StateMachineHandler.h
• src/handlers/StateMachineHandler.cpp