ApproachingMarkerState.h

Go to the documentation of this file.

 
#ifndef APPROACHING_MARKER_STATE_H
#define APPROACHING_MARKER_STATE_H
 
#include "../interfaces/State.hpp"
#include "../util/GeospatialOperations.hpp"
#include "../util/states/StuckDetection.hpp"
#include "../util/vision/TagDetectionUtilty.hpp"
#include "../vision/aruco/TagDetector.h"
 
#include <chrono>
 
 
 
namespace statemachine
{
 
    class ApproachingMarkerState : public State
    {
        private:
            // Core Components
            std::vector<std::shared_ptr<TagDetector>> m_vTagDetectors;
            statemachine::TimeIntervalBasedStuckDetector m_StuckDetector;
 
            // State tracking
            States m_eTriggeringState;
            bool m_bInitialized;
            geoops::Waypoint m_stGoalWaypoint;
 
            // Persistent tracking variables for Run()
            double m_dHeadingSetPoint;
            double m_dDistanceFromTag;
            bool m_bDriveBackwards;
 
            // Geolocation fallback
            geoops::Waypoint m_stLastGeolocatedPosition;
            bool m_bHasLastGeolocatedPosition;
            bool m_bHasSeenTarget;
 
            // Timing and logging flags
            std::chrono::system_clock::time_point m_tmLastSeenTime;
            std::chrono::system_clock::time_point m_tmLastLogTime;
            bool m_bAlreadyPrintedLost;
            bool m_bAlreadyPrintedVisualLostFallback;
 
        protected:
            void Start() override;
            void Exit() override;
 
        public:
            ApproachingMarkerState();
            void Run() override;
            States TriggerEvent(Event eEvent) override;
    };
}    // namespace statemachine
 
#endif    // APPROACHING_MARKER_STATE_H