StateMachineHandler.h

Go to the documentation of this file.

 
#ifndef STATEMACHINEHANDLER_H
#define STATEMACHINEHANDLER_H
 
#include "./CameraHandler.h"
 
#include "../states/ApproachingMarkerState.h"
#include "../states/ApproachingObjectState.h"
#include "../states/AvoidanceState.h"
#include "../states/IdleState.h"
#include "../states/NavigatingState.h"
#include "../states/ReversingState.h"
#include "../states/SearchPatternState.h"
#include "../states/StuckState.h"
#include "../states/VerifyingMarkerState.h"
#include "../states/VerifyingObjectState.h"
#include "../states/VerifyingPositionState.h"
 
#include <RoveComm/RoveComm.h>
#include <RoveComm/RoveCommManifest.h>
#include <atomic>
#include <shared_mutex>
 
 
 
class StateMachineHandler : private AutonomyThread<void>
{
    private:
        // Declare private class member variables.
        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;
 
        // Declare private class methods.
        std::shared_ptr<statemachine::State> CreateState(statemachine::States eState);
        void ChangeState(statemachine::States eNextState, const bool bSaveCurrentState = false);
        void SaveCurrentState();
        void ThreadedContinuousCode() override;
        void PooledLinearCode() override;
 
 
        const std::function<void(const rovecomm::RoveCommPacket<uint8_t>&, const sockaddr_in&)> AutonomyStartCallback =
            [this](const rovecomm::RoveCommPacket<uint8_t>& stPacket, const sockaddr_in& stdAddr)
        {
            // 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);
        };
 
 
        const std::function<void(const rovecomm::RoveCommPacket<uint8_t>&, const sockaddr_in&)> AutonomyStopCallback =
            [this](const rovecomm::RoveCommPacket<uint8_t>& stPacket, const sockaddr_in& stdAddr)
        {
            // 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);
        };
 
 
        const std::function<void(const rovecomm::RoveCommPacket<float>&, const sockaddr_in&)> PMSCellVoltageCallback =
            [this](const rovecomm::RoveCommPacket<float>& stPacket, const sockaddr_in& stdAddr)
        {
            // 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);
            }
        };
 
    public:
        // Declare public class methods and variables.
        StateMachineHandler();
        ~StateMachineHandler();
 
        void StartStateMachine();
        void StopStateMachine();
 
        void HandleEvent(statemachine::Event eEvent, const bool bSaveCurrentState = false);
 
        void ClearSavedStates();
        void ClearSavedState(statemachine::States eState);
        statemachine::States GetCurrentState() const;
        statemachine::States GetPreviousState() const;
 
        void RealignZEDPosition(CameraHandler::ZEDCamName eCameraName, const geoops::UTMCoordinate& stNewCameraPosition, const double dNewCameraHeading);
        using AutonomyThread::GetIPS;
};
 
#endif    // STATEMACHINEHANDLER_H