statemachine::SearchPatternState Class Reference

The SearchPatternState class implements the Search Pattern state for the Autonomy State Machine. More...

#include <SearchPatternState.h>

Inheritance diagram for statemachine::SearchPatternState:

Collaboration diagram for statemachine::SearchPatternState:

Public Member Functions
std::vector< geoops::Waypoint >	GeoPlanSearchPattern (const std::vector< geoops::Waypoint > &skeltonPath)
	This method takes in a skeleton path of waypoints and uses the GeoPlanner to create a new path that weaves through the skeleton path while avoiding obstacles.
void	RemoveRedZonePoints (std::vector< geoops::Waypoint > &skeltonPath)
	This method takes in a skeleton path of waypoints and removes any waypoints that are in red zones (obstacles) based on LiDAR data.
	SearchPatternState ()
	Construct a new State object.
void	Run () override
	Run the state machine. Returns the next state.
States	TriggerEvent (Event eEvent) override
	Trigger an event in the state machine. Returns the next state.
Public Member Functions inherited from statemachine::State
	State (States eState)
	Construct a new State object.
virtual	~State ()=default
	Destroy the State object.
States	GetState () const
	Accessor for the State private member.
virtual std::string	ToString () const
	Accessor for the State private member. Returns the state as a string.
virtual bool	operator== (const State &other) const
	Checks to see if the current state is equal to the passed state.
virtual bool	operator!= (const State &other) const
	Checks to see if the current state is not equal to the passed state.

Protected Member Functions
void	Start () override
	This method is called when the state is first started. It is used to initialize the state.
void	Exit () override
	This method is called when the state is exited. It is used to clean up the state.

Private Types
enum class	SearchPatternType { eSpiral , eSnake , eZigZag , END }

Private Attributes
bool	m_bWasStuck
bool	m_bInitialized
geoops::Waypoint	m_stSearchPatternCenter
std::vector< std::shared_ptr< TagDetector > >	m_vTagDetectors
std::vector< std::shared_ptr< ObjectDetector > >	m_vObjectDetectors
std::vector< geoops::Waypoint >	m_vSearchPath
int	m_nSearchPathIdx
SearchPatternType	m_eCurrentSearchPatternType
statemachine::TimeIntervalBasedStuckDetector	m_StuckDetector
std::unique_ptr< controllers::PurePursuitController >	m_pPursuitController

Detailed Description

The SearchPatternState class implements the Search Pattern state for the Autonomy State Machine.

Author

Eli Byrd (edbgk.nosp@m.k@ms.nosp@m.t.edu)

Date

2024-01-17

Member Enumeration Documentation

SearchPatternType

enum class statemachine::SearchPatternState::SearchPatternType

strongprivate

            {
                eSpiral,
                eSnake,
                eZigZag,
                END
            };

Constructor & Destructor Documentation

SearchPatternState()

statemachine::SearchPatternState::SearchPatternState

(

)

Construct a new State object.

Author

Eli Byrd (edbgk.nosp@m.k@ms.nosp@m.t.edu), Sam Hajdukiewicz (saman.nosp@m.thah.nosp@m.ajduk.nosp@m.iewi.nosp@m.cz@gm.nosp@m.ail..nosp@m.com)

Date

2024-01-17

                                           : State(States::eSearchPattern)
    {
        // Submit logger message.
        LOG_INFO(logging::g_qConsoleLogger, "Entering State: {}", ToString());
 
        // Initialize member variables.
        m_bInitialized       = false;
        m_StuckDetector      = statemachine::TimeIntervalBasedStuckDetector(constants::SEARCH_STUCK_CHECK_ATTEMPTS, constants::SEARCH_STUCK_CHECK_INTERVAL);
        m_pPursuitController = std::make_unique<controllers::PurePursuitController>();
 
        // Start state.
        if (!m_bInitialized)
        {
            Start();
            m_bInitialized = true;
        }
    }

Here is the call graph for this function:

Member Function Documentation

Start()

void statemachine::SearchPatternState::Start ( )

overrideprotectedvirtual

This method is called when the state is first started. It is used to initialize the state.

Author

Eli Byrd (edbgk.nosp@m.k@ms.nosp@m.t.edu), Sam Hajdukiewicz (saman.nosp@m.thah.nosp@m.ajduk.nosp@m.iewi.nosp@m.cz@gm.nosp@m.ail..nosp@m.com)

Date

2024-01-17

Reimplemented from statemachine::State.

    {
        // Schedule the next run of the state's logic
        LOG_INFO(logging::g_qSharedLogger, "SearchPatternState: Scheduling next run of state logic.");
 
        // Initialize member variables.
        m_bWasStuck                 = false;
        m_eCurrentSearchPatternType = SearchPatternType::eSpiral;
        m_nSearchPathIdx            = 0;
        m_stSearchPatternCenter     = globals::g_pWaypointHandler->PeekNextWaypoint();
 
        // Get the current rover pose.
        geoops::RoverPose stCurrentRoverPose = globals::g_pStateMachineHandler->SmartRetrieveRoverPose();
 
        // Calculate the search path.
        std::vector<geoops::Waypoint> vSpiralPath = searchpattern::CalculateSpiralPatternWaypoints(m_stSearchPatternCenter.GetGPSCoordinate(),
                                                                                                   constants::SEARCH_ANGULAR_STEP_DEGREES,
                                                                                                   m_stSearchPatternCenter.dRadius,
                                                                                                   stCurrentRoverPose.GetCompassHeading(),
                                                                                                   constants::SEARCH_SPIRAL_SPACING);
        RemoveRedZonePoints(vSpiralPath);
        std::vector<geoops::Waypoint> vGeoPlannedPath = GeoPlanSearchPattern(vSpiralPath);
 
        // Split the path into two halves for forward and reverse navigation.
        std::vector<geoops::Waypoint> vFirstHalf;
        std::vector<geoops::Waypoint> vSecondHalf;
        if (vGeoPlannedPath.size() >= 4)
        {
            vFirstHalf  = std::vector<geoops::Waypoint>(vGeoPlannedPath.begin(), vGeoPlannedPath.begin() + vGeoPlannedPath.size() / 2);
            vSecondHalf = std::vector<geoops::Waypoint>(vGeoPlannedPath.begin() + vGeoPlannedPath.size() / 2, vGeoPlannedPath.end());
        }
        else if (vGeoPlannedPath.size() >= 2)
        {
            vFirstHalf = vGeoPlannedPath;
        }
 
        // Plot the search path in the visualizer.
        globals::g_pWaypointHandler->StorePath("GeoPlannerPath", vFirstHalf);
        globals::g_pWaypointHandler->StorePath("GeoPlannerPathReverse", vSecondHalf);
        m_vSearchPath = std::move(vFirstHalf);
 
        // Set the path of the pure pursuit controller.
        m_pPursuitController->SetReferencePath(m_vSearchPath);
        m_pPursuitController->SetLookaheadIndex(5);
 
        m_vTagDetectors    = {globals::g_pTagDetectionHandler->GetTagDetector(TagDetectionHandler::TagDetectors::eHeadMainCam),
                              globals::g_pTagDetectionHandler->GetTagDetector(TagDetectionHandler::TagDetectors::eRearCam)};
        m_vObjectDetectors = {globals::g_pObjectDetectionHandler->GetObjectDetector(ObjectDetectionHandler::ObjectDetectors::eHeadMainCam),
                              globals::g_pObjectDetectionHandler->GetObjectDetector(ObjectDetectionHandler::ObjectDetectors::eRearCam)};
    }

Here is the call graph for this function:

Here is the caller graph for this function:

Exit()

void statemachine::SearchPatternState::Exit ( )

overrideprotectedvirtual

This method is called when the state is exited. It is used to clean up the state.

Author

Eli Byrd (edbgk.nosp@m.k@ms.nosp@m.t.edu)

Date

2024-01-17

Reimplemented from statemachine::State.

    {
        // Clean up the state before exiting
        LOG_INFO(logging::g_qSharedLogger, "SearchPatternState: Exiting state.");
 
        // Stop drive.
        globals::g_pDriveBoard->SendStop();
    }

Here is the call graph for this function:

Here is the caller graph for this function:

GeoPlanSearchPattern()

std::vector< geoops::Waypoint > statemachine::SearchPatternState::GeoPlanSearchPattern

(

const std::vector< geoops::Waypoint > &

vSkeletonPath

)

This method takes in a skeleton path of waypoints and uses the GeoPlanner to create a new path that weaves through the skeleton path while avoiding obstacles.

Parameters

vSkeletonPath

- The input skeleton path of waypoints that the search pattern should weave through.

Returns

std::vector<geoops::Waypoint> - The new path generated by the GeoPlanner that weaves through the skeleton path while avoiding obstacles.

Author

Hunter LeRette (hrlnp.nosp@m.c@ms.nosp@m.t.edu), Jordan Hoover (jh69n.nosp@m.@mst.nosp@m..edu), Aiden Buter (ab9hm.nosp@m.@mst.nosp@m..edu)

Date

2026-05-17

    {
        std::vector<geoops::Waypoint> vPlannedPath;
        if (vSkeletonPath.size() < 2)
        {
            return vPlannedPath;
        }
        for (long unsigned int nI = 0; nI < vSkeletonPath.size() - 1; nI++)
        {
            std::vector<geoops::Waypoint> vNewPoints =
                globals::g_pGeoPlanner->PlanPath(globals::g_pLiDARHandler, vSkeletonPath[nI].GetUTMCoordinate(), vSkeletonPath[nI + 1].GetUTMCoordinate());
            vPlannedPath.insert(vPlannedPath.end(), vNewPoints.begin(), vNewPoints.end());
        }
 
        return vPlannedPath;
    }

Here is the call graph for this function:

Here is the caller graph for this function:

RemoveRedZonePoints()

void statemachine::SearchPatternState::RemoveRedZonePoints

(

std::vector< geoops::Waypoint > &

vSkeletonPath

)

This method takes in a skeleton path of waypoints and removes any waypoints that are in red zones (obstacles) based on LiDAR data.

Parameters

vSkeletonPath

- The input skeleton path of waypoints that the search pattern should weave through.

Author

Hunter LeRette (hrlnp.nosp@m.c@ms.nosp@m.t.edu), Jordan Hoover (jh69n.nosp@m.@mst.nosp@m..edu), Aiden Buter (ab9hm.nosp@m.@mst.nosp@m..edu)

Date

2026-05-17

    {
        for (long unsigned int i = 0; i < vSkeletonPath.size();)
        {
            int nTileX = static_cast<int>(std::floor(vSkeletonPath[i].GetUTMCoordinate().dEasting / 5.0));
            int nTileY = static_cast<int>(std::floor(vSkeletonPath[i].GetUTMCoordinate().dNorthing / 5.0));
            LiDARHandler::PointFilter stFilter;
            stFilter.dEasting                              = (nTileX + 0.5) * 5.0;                                  // Center of the tile in easting.
            stFilter.dNorthing                             = (nTileY + 0.5) * 5.0;                                  // Center of the tile in northing.
            stFilter.dRadius                               = std::sqrt(2) * (5.0 / 2.0);                            // Radius to cover the entire tile
            stFilter.dTraversalScore                       = LiDARHandler::PointFilter::Range<double>{0.5, 1.0};    // Only load points with sufficient traversal
 
            std::vector<LiDARHandler::PointRow> vLidarData = globals::g_pLiDARHandler->GetLiDARData(stFilter);
 
            if (vLidarData.empty())
            {
                vSkeletonPath.erase(vSkeletonPath.begin() + static_cast<long int>(i));
            }
            else
            {
                ++i;
            }
        }
    }

Here is the call graph for this function:

Here is the caller graph for this function:

Run()

void statemachine::SearchPatternState::Run ( )

overridevirtual

Run the state machine. Returns the next state.

Author

Jason Pittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-01-17

Implements statemachine::State.

    {
        // Submit logger message.
        LOG_DEBUG(logging::g_qSharedLogger, "SearchPatternState: Running state-specific behavior.");
 
        // If search was previously stuck, then re-path plan stuck area
        if (m_bWasStuck)
        {
            // Retrieve modified path from stuck.
            m_vSearchPath = globals::g_pWaypointHandler->RetrievePath("GeoPlannerPath");
 
            // Update visualizer and pure pursuit
            m_pPursuitController->SetReferencePath(m_vSearchPath);
 
            m_bWasStuck = false;
        }
 
        // Get the current rover pose.
        geoops::RoverPose stCurrentRoverPose = globals::g_pStateMachineHandler->SmartRetrieveRoverPose();
 
        /*
            The overall flow of this state is as follows.
            1. Is there a tag -> MarkerSeen
            2. Is there an object -> ObjectSeen
            3. Is there an obstacle -> TBD
            4. Is the rover stuck -> Stuck
            5. Is the search pattern complete -> Abort
            6. Follow the search pattern.
        */
 
        /* --- Detect Tags --- */
 
        // In order to even care about any tags we see, the goal waypoint needs to be of type MARKER and we need to be within the search radius of the MARKER waypoint.
        if (m_stSearchPatternCenter.eType == geoops::WaypointType::eTagWaypoint)
        {
            // Create instance variables.
            tagdetectutils::ArucoTag stBestArucoTag, stBestTorchTag;
            // Identify target marker.
            statemachine::IdentifyTargetMarker(m_vTagDetectors, stBestArucoTag, stBestTorchTag, m_stSearchPatternCenter.nID);
            // Check if either tag type is seen.
            if (stBestArucoTag.nID != -1 || stBestTorchTag.dConfidence != 0.0)
            {
                // Submit logger message.
                LOG_NOTICE(logging::g_qSharedLogger, "SearchPatternState: Rover has seen a target marker!");
 
                // Handle state transition and save the current search pattern state.
                globals::g_pStateMachineHandler->HandleEvent(Event::eMarkerSeen, true);
                // Don't execute the rest of the state.
                return;
            }
        }
 
        /* --- Detect Objects --- */
 
        // In order to even care about any objects we see, the goal waypoint needs to be of an object type and we need to be within the search radius of the object
        // waypoint.
        if (m_stSearchPatternCenter.eType == geoops::WaypointType::eObjectWaypoint || m_stSearchPatternCenter.eType == geoops::WaypointType::eMalletWaypoint ||
            m_stSearchPatternCenter.eType == geoops::WaypointType::eWaterBottleWaypoint || m_stSearchPatternCenter.eType == geoops::WaypointType::eRockPickWaypoint)
        {
            // Create instance variables.
            objectdetectutils::Object stBestTorchObject;
            // Identify target object.
            statemachine::IdentifyTargetObject(m_vObjectDetectors, stBestTorchObject, m_stSearchPatternCenter.eType);
            // Check if either tag type is seen.
            if (stBestTorchObject.dConfidence != 0.0)
            {
                // Submit logger message.
                LOG_NOTICE(logging::g_qSharedLogger, "SearchPatternState: Rover has seen a target object!");
 
                // Handle state transition and save the current search pattern state.
                globals::g_pStateMachineHandler->HandleEvent(Event::eObjectSeen, true);
                // Don't execute the rest of the state.
                return;
            }
        }
 
        /* --- Detect Obstacles --- */
 
        // TODO: Add obstacle detection to SearchPattern state
 
        /* ---  Check if the rover is stuck --- */
 
        // Check if stuck.
        if (constants::SEARCH_ENABLE_STUCK_DETECT && m_StuckDetector.CheckIfStuck(globals::g_pStateMachineHandler->SmartRetrieveVelocity(),
                                                                                  globals::g_pStateMachineHandler->SmartRetrieveAngularVelocity(),
                                                                                  constants::SEARCH_STUCK_CHECK_VEL_THRESH * globals::g_pDriveBoard->GetMaxDriveEffort(),
                                                                                  constants::SEARCH_STUCK_CHECK_ROT_THRESH))
        {
            // Submit logger message.
            LOG_WARNING(logging::g_qSharedLogger, "SearchPattern: Rover has become stuck!");
            m_bWasStuck = true;
            // Handle state transition and save the current search pattern state.
            globals::g_pStateMachineHandler->HandleEvent(Event::eStuck, true);
            // Don't execute the rest of the state.
            return;
        }
 
        /* --- Follow Search Pattern --- */
 
        // Check if the search path has enough points to navigate.
        if (m_vSearchPath.size() < 2)
        {
            // Submit logger message.
            LOG_WARNING(logging::g_qSharedLogger, "SearchPatternState: Search path has fewer than 2 points, aborting search.");
            // Handle state transition.
            globals::g_pStateMachineHandler->HandleEvent(Event::eAbort);
            return;
        }
 
        // Have we reached the final waypoint of the search pattern?
        geoops::GPSCoordinate stFinalTargetGPS    = m_vSearchPath.back().GetGPSCoordinate();
        geoops::GeoMeasurement stRelToFinalTarget = geoops::CalculateGeoMeasurement(stCurrentRoverPose.GetGPSCoordinate(), stFinalTargetGPS);
        double dCompletionRadius                  = constants::SEARCH_WAYPOINT_PROXIMITY;
        bool bReachedFinalTarget                  = stRelToFinalTarget.dDistanceMeters <= dCompletionRadius;
 
        // If the entire search pattern has been completed without seeing tags or objects, try different search pattern.
        if (bReachedFinalTarget && m_pPursuitController->GetReferencePathTargetIndex() > static_cast<int>(m_vSearchPath.size()) - 4)
        {
            globals::g_pStateMachineHandler->HandleEvent(Event::eSearchFailed);
            return;
        }
 
        // NOTE: Optional - Uncomment the above code and comment out the below code to use pure pursuit control to navigate to the goal waypoint.
        // Use pure pursuit to calculate drive move/powers.
        controllers::PurePursuitController::DriveVector stDriveVector = m_pPursuitController->Calculate(stCurrentRoverPose, constants::SEARCH_MOTOR_POWER);
        // Calculate move from goal heading and desired speed.
        diffdrive::DrivePowers stDriveSpeeds = globals::g_pDriveBoard->CalculateMove(stDriveVector.dVelocity,
                                                                                     stDriveVector.dThetaHeading,
                                                                                     stCurrentRoverPose.GetCompassHeading(),
                                                                                     diffdrive::DifferentialControlMethod::eArcadeDrive);
        // Send drive powers over RoveComm.
        globals::g_pDriveBoard->SendDrive(stDriveSpeeds);
 
        return;
    }

Here is the call graph for this function:

TriggerEvent()

States statemachine::SearchPatternState::TriggerEvent ( Event  eEvent )

overridevirtual

Trigger an event in the state machine. Returns the next state.

Parameters

eEvent

- The event to trigger.

Returns

std::shared_ptr<State> - The next state.

Author

Eli Byrd (edbgk.nosp@m.k@ms.nosp@m.t.edu)

Date

2024-01-17

Implements statemachine::State.

    {
        // Create instance variables.
        States eNextState       = States::eSearchPattern;
        bool bCompleteStateExit = true;
 
        switch (eEvent)
        {
            case Event::eMarkerSeen:
            {
                // Submit logger message.
                LOG_INFO(logging::g_qSharedLogger, "SearchPatternState: Handling MarkerSeen event.");
                // Change states.
                eNextState = States::eApproachingMarker;
                break;
            }
            case Event::eObjectSeen:
            {
                // Submit logger message.
                LOG_INFO(logging::g_qSharedLogger, "SearchPatternState: Handling ObjectSeen event.");
                // Change state.
                eNextState = States::eApproachingObject;
                break;
            }
            case Event::eStart:
            {
                // Submit logger message
                LOG_NOTICE(logging::g_qSharedLogger, "SearchPatternState: Handling Start event.");
                // Send multimedia command to update state display.
                globals::g_pMultimediaBoard->SendLightingState(MultimediaBoard::MultimediaBoardLightingState::eAutonomy);
                break;
            }
            case Event::eSearchFailed:
            {
                // Submit logger message.
                LOG_INFO(logging::g_qSharedLogger, "SearchPatternState: Handling SearchFailed event.");
                // Stop drive.
                globals::g_pDriveBoard->SendStop();
 
                // Regenerate a new search pattern.
                switch (m_eCurrentSearchPatternType)
                {
                    // Check which pattern to do next.
                    case SearchPatternType::eSpiral:
                    {
                        // Submit logger message.
                        LOG_NOTICE(logging::g_qSharedLogger, "SearchPatternState: Spiral search pattern failed, trying reverse spiral...");
 
                        // Reset index counter.
                        m_nSearchPathIdx = 0;
                        // Update current search pattern
                        m_eCurrentSearchPatternType = SearchPatternType::END;
 
                        // Get the second half of the spiral.
                        m_vSearchPath = globals::g_pWaypointHandler->RetrievePath("GeoPlannerPathReverse");
                        globals::g_pWaypointHandler->DeletePath("GeoPlannerPathReverse");
 
                        if (m_vSearchPath.size() < 2)
                        {
                            LOG_WARNING(logging::g_qSharedLogger, "SearchPatternState: Reverse search path has fewer than 2 points, giving up...");
                            globals::g_pWaypointHandler->PopNextWaypoint();
                            eNextState = States::eIdle;
                            break;
                        }
 
                        // Plot the search path in the visualizer.
                        globals::g_pWaypointHandler->StorePath("GeoPlannerPath", m_vSearchPath);
                        // Set the path of the pure pursuit controller.
                        m_pPursuitController->SetReferencePath(m_vSearchPath);
                        break;
                    }
                    case SearchPatternType::END:
                    {
                        // Submit logger message.
                        LOG_WARNING(logging::g_qSharedLogger, "SearchPatternState: All patterns failed to find anything, giving up...");
                        // Pop old waypoint out of queue.
                        globals::g_pWaypointHandler->PopNextWaypoint();
                        // Change states.
                        eNextState = States::eIdle;
                        break;
                    }
                    default:
                    {
                        // Change states.
                        eNextState = States::eIdle;
                        break;
                    }
                }
                break;
            }
            case Event::eAbort:
            {
                // Submit logger message.
                LOG_INFO(logging::g_qSharedLogger, "SearchPatternState: Handling Abort event.");
                // Send multimedia command to update state display.
                globals::g_pMultimediaBoard->SendLightingState(MultimediaBoard::MultimediaBoardLightingState::eOff);
                // Stop drive.
                globals::g_pDriveBoard->SendStop();
                // Change state.
                eNextState = States::eIdle;
                break;
            }
            case Event::eStuck:
            {
                LOG_INFO(logging::g_qSharedLogger, "SearchPatternState: Handling Stuck event.");
                eNextState = States::eStuck;
                break;
            }
            default:
            {
                LOG_WARNING(logging::g_qSharedLogger, "SearchPatternState: Handling unknown event.");
                eNextState = States::eIdle;
                break;
            }
        }
 
        if (eNextState != States::eSearchPattern)
        {
            LOG_INFO(logging::g_qSharedLogger, "SearchPatternState: Transitioning to {} State.", StateToString(eNextState));
 
            // Exit the current state
            if (bCompleteStateExit)
            {
                Exit();
            }
        }
 
        return eNextState;
    }

Here is the call graph for this function:

---

The documentation for this class was generated from the following files:

• src/states/SearchPatternState.h
• src/states/SearchPatternState.cpp