statemachine::AvoidanceState Class Reference

The AvoidanceState class implements the Avoidance state for the Autonomy State Machine. More...

#include <AvoidanceState.h>

Inheritance diagram for statemachine::AvoidanceState:

Collaboration diagram for statemachine::AvoidanceState:

Public Member Functions
	AvoidanceState ()
	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 Attributes
statemachine::TimeIntervalBasedStuckDetector	m_stStuckChecker
pathplanners::AStar	m_stPlanner
controllers::StanleyController	m_stController
geoops::RoverPose	m_stPose
geoops::UTMCoordinate	m_stStart
geoops::UTMCoordinate	m_stGoal
geoops::Waypoint	m_stGoalWaypoint
std::vector< geoops::UTMCoordinate >	m_vPlannedRoute
States	m_eTriggeringState
bool	m_bInitialized

Detailed Description

The AvoidanceState class implements the Avoidance state for the Autonomy State Machine.

Author

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

Date

2024-01-17

Constructor & Destructor Documentation

AvoidanceState()

statemachine::AvoidanceState::AvoidanceState

(

)

Construct a new State object.

Author

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

Date

2024-01-17

                                   : State(States::eAvoidance)
    {
        LOG_INFO(logging::g_qConsoleLogger, "Entering State: {}", ToString());
 
        m_bInitialized   = false;
        m_stStuckChecker = statemachine::TimeIntervalBasedStuckDetector(constants::STUCK_CHECK_ATTEMPTS,
                                                                        constants::STUCK_CHECK_INTERVAL,
                                                                        constants::STUCK_CHECK_VEL_THRESH,
                                                                        constants::STUCK_CHECK_ROT_THRESH);
 
        if (!m_bInitialized)
        {
            Start();
            m_bInitialized = true;
        }
    }

Here is the call graph for this function:

Member Function Documentation

Start()

void statemachine::AvoidanceState::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)

Date

2024-01-17

Reimplemented from statemachine::State.

    {
        // Schedule the next run of the state's logic
        LOG_INFO(logging::g_qSharedLogger, "AvoidanceState: Scheduling next run of state logic.");
 
        // Store the state that got stuck and triggered a stuck event.
        m_eTriggeringState = globals::g_pStateMachineHandler->GetPreviousState();
 
        // Initialize Stanley Controller:
        m_stController = controllers::StanleyController(constants::STANLEY_STEER_CONTROL_GAIN, constants::STANLEY_DIST_TO_FRONT_AXLE, constants::STANLEY_YAW_TOLERANCE);
 
        // Initialize ASTAR Pathfinder:
        // TODO: Poll zedCam / object detector for seen obstacles to pass to AStar.
        // Determine start and goal (peek waypoint for goal).
        m_stPose         = globals::g_pWaypointHandler->SmartRetrieveRoverPose();
        m_stStart        = m_stPose.GetUTMCoordinate();
        m_stGoalWaypoint = globals::g_pWaypointHandler->PeekNextWaypoint();
 
        // Plan avoidance route using AStar.
        // TODO: Add obstacles to params.
        m_vPlannedRoute = m_stPlanner.PlanAvoidancePath(m_stStart, m_stGoalWaypoint.GetUTMCoordinate());
 
        // Check for AStar failure.
        if (!m_vPlannedRoute.empty())
        {
            m_stGoal = m_vPlannedRoute.back();
            m_stController.SetPath(m_vPlannedRoute);
        }
        // Exit Obstacle Avoidance if AStar fails to generate a path.
        else
        {
            globals::g_pStateMachineHandler->HandleEvent(Event::eEndObstacleAvoidance, false);
        }
    }

Here is the call graph for this function:

Here is the caller graph for this function:

Exit()

void statemachine::AvoidanceState::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, "AvoidanceState: Exiting state.");
 
        // Clear ASTAR Pathfinder
        m_stPlanner.ClearObstacleData();
 
        // Clear Stanley Controller
        m_stController.ClearPath();
    }

Here is the call graph for this function:

Here is the caller graph for this function:

Run()

void statemachine::AvoidanceState::Run ( )

overridevirtual

Run the state machine. Returns the next state.

Author

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

Date

2024-01-17

Implements statemachine::State.

    {
        LOG_DEBUG(logging::g_qSharedLogger, "AvoidanceState: Running state-specific behavior.");
 
        // TODO: build in obstacle detection and refresh of astar path when a new object is detected
        // This can be done by calling PlanAvoidanceRoute() with an updated obstacle vector.
 
        // A route has already been plotted by the planner and passed to the controller.
        // Navigate by issuing drive commands from the controller.
        // Check if we are at the goal waypoint.
        geoops::RoverPose stCurrentPose                  = globals::g_pWaypointHandler->SmartRetrieveRoverPose();
        geoops::GeoMeasurement stGoalWaypointMeasurement = geoops::CalculateGeoMeasurement(stCurrentPose.GetUTMCoordinate(), m_stGoalWaypoint.GetUTMCoordinate());
 
        // Check to see if rover velocity is below stuck threshold (scaled to avoidance speed).
        if (m_stStuckChecker.CheckIfStuck(globals::g_pWaypointHandler->SmartRetrieveVelocity(), globals::g_pWaypointHandler->SmartRetrieveAngularVelocity()))
        {
            LOG_INFO(logging::g_qSharedLogger, "AvoidanceState: Rover has become stuck");
            globals::g_pStateMachineHandler->HandleEvent(Event::eStuck, true);
        }
 
        // Goal has not been reached yet:
        else if (stGoalWaypointMeasurement.dDistanceMeters > constants::NAVIGATING_REACHED_GOAL_RADIUS)
        {
            // Request objects:
            // Check for any new objects:
            // Re-plan route (call planPath again):
 
            // Update pose for drive calculation.
            m_stPose               = globals::g_pWaypointHandler->SmartRetrieveRoverPose();
            const double dVelocity = globals::g_pWaypointHandler->SmartRetrieveVelocity();
            // Calculate drive move/powers at the speed multiplier.
            diffdrive::DrivePowers stDriveSpeeds =
                globals::g_pDriveBoard->CalculateMove(constants::AVOIDANCE_STATE_MOTOR_POWER,
                                                      m_stController.Calculate(m_stPose.GetUTMCoordinate(), dVelocity, m_stPose.GetCompassHeading()),
                                                      stCurrentPose.GetCompassHeading(),
                                                      diffdrive::DifferentialControlMethod::eArcadeDrive);
            // Send drive command to drive board.
            globals::g_pDriveBoard->SendDrive(stDriveSpeeds);
        }
 
        // Local goal is reached, end obstacle avoidance:
        else
        {
            globals::g_pDriveBoard->SendStop();
            globals::g_pStateMachineHandler->HandleEvent(Event::eEndObstacleAvoidance, false);
        }
    }

Here is the call graph for this function:

TriggerEvent()

States statemachine::AvoidanceState::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::eAvoidance;
        bool bCompleteStateExit = true;
 
        switch (eEvent)
        {
            case Event::eStart:
            {
                // Submit logger message.
                LOG_INFO(logging::g_qSharedLogger, "AvoidanceState: Handling Start event.");
                // Send multimedia command to update state display.
                globals::g_pMultimediaBoard->SendLightingState(MultimediaBoard::MultimediaBoardLightingState::eAutonomy);
                break;
            }
            case Event::eAbort:
            {
                // Submit logger message.
                LOG_INFO(logging::g_qSharedLogger, "AvoidanceState: Handling Abort event.");
                // Send multimedia command to update state display.
                globals::g_pMultimediaBoard->SendLightingState(MultimediaBoard::MultimediaBoardLightingState::eAutonomy);
                // Change state.
                eNextState = States::eIdle;
                break;
            }
            case Event::eEndObstacleAvoidance:
            {
                LOG_INFO(logging::g_qSharedLogger, "AvoidanceState: Handling EndObstacleAvoidance event.");
                eNextState = m_eTriggeringState;
                break;
            }
            case Event::eStuck:
            {
                LOG_INFO(logging::g_qSharedLogger, "AvoidanceState: Handling Stuck event.");
                eNextState = States::eStuck;
                break;
            }
            default:
            {
                LOG_WARNING(logging::g_qSharedLogger, "AvoidanceState: Handling unknown event.");
                eNextState = States::eIdle;
                break;
            }
        }
 
        if (eNextState != States::eAvoidance)
        {
            LOG_INFO(logging::g_qSharedLogger, "AvoidanceState: 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/AvoidanceState.h
• src/states/AvoidanceState.cpp