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 bIMUHeading=true)
	This method is used to retrieve the rover's current position and heading. It uses the GPS data from the NavBoard and if enabled, it can also fuse the ZEDCam's IMU heading data for a more accurate heading estimation. The method returns a RoverPose struct that contains the current GPS coordinate and the fused heading of the rover.
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	RealignZEDHeading (const double dNewActualHeading, const double dCurrentZEDHeading)
	This method is used to realign the ZED camera's heading with the actual heading of the rover. This method calculates the difference between the ZED's heading and the actual heading of the rover and applies that difference as an offset to the ZED's heading.
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.
std::string	GetThreadUUID () const
	Accessor for the Thread U U I D private member.
IPS &	GetIPS ()
	Accessor for the Frame I P S private member.
void	SetMainThreadPriority (AutonomyThreadPriority ePriority)
	Set the OS priority for the main continuous thread.
void	SetPoolThreadPriority (AutonomyThreadPriority ePriority)
	Set the OS priority for the highly parallelized pool threads.
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
std::shared_ptr< ZEDCamera >	m_pRearCam
geoops::GPSCoordinate	m_stCurrentGPSLocation
double	m_dZEDHeadingOffset
double	m_dLastRawZEDHeading
double	m_dLastFusedHeading
bool	m_bFirstHeadingLoop
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
enum	AutonomyThreadPriority

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);
    m_pRearCam           = globals::g_pCameraHandler->GetZED(CameraHandler::ZEDCamName::eRearCam);
    m_dZEDHeadingOffset  = 0.0;
    m_dLastRawZEDHeading = 0.0;
    m_dLastFusedHeading  = 0.0;
    m_bFirstHeadingLoop  = true;
 
    // 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::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();
    }
}

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

bIMUHeading = true

)

This method is used to retrieve the rover's current position and heading. It uses the GPS data from the NavBoard and if enabled, it can also fuse the ZEDCam's IMU heading data for a more accurate heading estimation. The method returns a RoverPose struct that contains the current GPS coordinate and the fused heading of the rover.

Parameters

bIMUHeading

- Whether to use IMU Heading.

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.
    double dFusedHeading = dCurrentGPSHeading;
 
    if ((bIMUHeading) && !constants::MODE_SIM && m_pMainCam->GetCameraIsOpen())
    {
        // DYNAMIC REALIGNMENT. (Drift/Lag correction using GPS)
        double dVelocity   = this->SmartRetrieveVelocity();
        double dAngularVel = this->SmartRetrieveAngularVelocity();
 
        // Request the current heading from the ZED camera.
        sl::SensorsData slCurrentCameraSensorData;
        std::future<bool> fuResultStatus = m_pMainCam->RequestSensorsCopy(slCurrentCameraSensorData);
        // Wait for future to be fulfilled.
        if (fuResultStatus.get())
        {
            // Get Degrees heading from ZED IMU data.
            double dCurrentZEDHeading = slCurrentCameraSensorData.imu.pose.getEulerAngles(false).y;
            // Realign offset.
            // If driving forward fast enough (> Xm/s) and NOT turning. (angular vel near 0)
            if ((m_pCurrentState != nullptr && m_pCurrentState->GetState() == statemachine::States::eIdle) ||
                (std::abs(dVelocity) > constants::ZED_REALIGN_VEL_THRESH && std::abs(dAngularVel) < constants::ZED_REALIGN_ROT_THRESH))
            {
                this->RealignZEDHeading(dCurrentGPSHeading, dCurrentZEDHeading);
            }
 
            // Update fused heading.
            dFusedHeading = numops::InputAngleModulus(dCurrentZEDHeading + m_dZEDHeadingOffset, 0.0, 360.0);
        }
    }
 
    // Submit a debug print for the current rover pose.
    geoops::UTMCoordinate stCurrentUTMPosition = geoops::ConvertGPSToUTM(stCurrentGPSPosition);
    LOG_DEBUG(logging::g_qSharedLogger,
              "Rover Pose is currently: {} (easting), {} (northing), {} (alt), {} (degrees), IMUHeading = {}",
              stCurrentUTMPosition.dEasting,
              stCurrentUTMPosition.dNorthing,
              stCurrentUTMPosition.dAltitude,
              dFusedHeading,
              bIMUHeading ? "true" : "false");
 
    return geoops::RoverPose(stCurrentGPSPosition, dFusedHeading);
}

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:

RealignZEDHeading()

void StateMachineHandler::RealignZEDHeading	(	const double	dNewActualHeading,
		const double	dCurrentZEDHeading
	)

This method is used to realign the ZED camera's heading with the actual heading of the rover. This method calculates the difference between the ZED's heading and the actual heading of the rover and applies that difference as an offset to the ZED's heading.

Parameters

dNewActualHeading	- The new actual heading of the rover that the ZED's heading should be aligned to.
dCurrentZEDHeading	- The current heading of the zed.

Author

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

Date

2024-04-17

{
    // Convert -180/180 to 0/360 Standard. (Keep 0 as 0)
    // If ZED is -90 (West), this makes it 270.
    double dCurrentHeading = dCurrentZEDHeading;
    if (dCurrentHeading < 0)
    {
        dCurrentHeading += 360.0;
    }
 
    // Calculate the difference required to turn Raw ZED into Actual Heading.
    // Logic: Actual = Raw + Offset  =>  Offset = Actual - Raw
    double dOffset = dNewActualHeading - dCurrentHeading;
 
    // Wrap the offset to 0-360 positive range
    dOffset = numops::InputAngleModulus(dOffset, 0.0, 360.0);
 
    // Normalize the signed offset to the range [-180, 180] so we report the smallest correction.
    double dSignedOffset = numops::AngularDifference(m_dZEDHeadingOffset, dOffset);
    // Only print a notice when the correction exceeds the configured significant threshold.
    if (std::abs(dSignedOffset) >= constants::ZED_REALIGN_ROT_THRESH)
    {
        LOG_NOTICE(logging::g_qSharedLogger,
                   "Significant ZED heading correction detected. Raw ZED: {} deg, Target GPS: {} deg, Signed Offset: {} deg, New Offset: {} deg",
                   dCurrentHeading,
                   dNewActualHeading,
                   dSignedOffset,
                   m_dZEDHeadingOffset);
    }
 
    // Update zed offset.
    m_dZEDHeadingOffset = dOffset;
}

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