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.
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
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< 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<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;
    }
}

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 GNSS Fusion is enabled and the main ZED camera is a fusion master.
        if (constants::FUSION_ENABLE_GNSS_FUSION && 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;
 
    // 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);
 
    // 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);
}

Here is the caller graph for this function:

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();
}

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:

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.
    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);
        };

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