StateMachineHandler.h

Go to the documentation of this file.

 
#ifndef STATEMACHINEHANDLER_H
#define STATEMACHINEHANDLER_H
 
#include "../states/ApproachingMarkerState.h"
#include "../states/ApproachingObjectState.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 "./CameraHandler.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;
        std::shared_ptr<ZEDCamera> m_pMainCam;
        std::shared_ptr<ZEDCamera> m_pRearCam;
        geoops::GPSCoordinate m_stCurrentGPSLocation;
        double m_dZEDHeadingOffset;    // This is the offset that is applied to the ZED's heading to align it with the actual heading of the rover.
 
        // Kinematic tracking variables for heading recovery.
        double m_dLastRawZEDHeading;
        double m_dLastFusedHeading;
        bool m_bFirstHeadingLoop;
 
        // 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<uint8_t>&, const sockaddr_in&)> ClearWaypointsCallback =
            [this](const rovecomm::RoveCommPacket<uint8_t>& stPacket, const sockaddr_in& stdAddr)
        {
            // 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()));
            }
        };
 
 
        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;
 
        // Smart location retrieving.
        geoops::RoverPose SmartRetrieveRoverPose(bool bIMUHeading = true);
        double SmartRetrieveVelocity();
        double SmartRetrieveAngularVelocity();
        void RealignZEDHeading(const double dNewActualHeading, const double dCurrentZEDHeading);
 
        using AutonomyThread::GetIPS;
};
 
#endif    // STATEMACHINEHANDLER_H