Autonomy Software C++ 24.5.1
Welcome to the Autonomy Software repository of the Mars Rover Design Team (MRDT) at Missouri University of Science and Technology (Missouri S&T)! API reference contains the source code and other resources for the development of the autonomy software for our Mars rover. The Autonomy Software project aims to compete in the University Rover Challenge (URC) by demonstrating advanced autonomous capabilities and robust navigation algorithms.
Loading...
Searching...
No Matches
statemachine::NavigatingState Class Reference

The NavigatingState class implements the Navigating state for the Autonomy State Machine. More...

#include <NavigatingState.h>

Inheritance diagram for statemachine::NavigatingState:
Collaboration diagram for statemachine::NavigatingState:

Public Member Functions

 NavigatingState ()
 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

bool m_bFetchNewWaypoint
 
geoops::Waypoint m_stGoalWaypoint
 
bool m_bInitialized
 
std::vector< std::shared_ptr< TagDetector > > m_vTagDetectors
 
std::vector< std::shared_ptr< ObjectDetector > > m_vObjectDetectors
 
statemachine::TimeIntervalBasedStuckDetector m_StuckDetector
 
std::unique_ptr< logging::graphing::PathTracerm_pRoverPathPlot
 
std::unique_ptr< controllers::PredictiveStanleyControllerm_pStanleyController
 
std::unique_ptr< pathplanners::AStarm_pAStarPlanner
 
std::vector< geoops::Waypointm_vPathCoordinates
 

Detailed Description

The NavigatingState class implements the Navigating 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

◆ NavigatingState()

statemachine::NavigatingState::NavigatingState ( )

Construct a new State object.

Author
Eli Byrd (edbgk.nosp@m.k@ms.nosp@m.t.edu)
Date
2024-01-17
74 : State(States::eNavigating)
75 {
76 // Submit logger message.
77 LOG_INFO(logging::g_qConsoleLogger, "Entering State: {}", ToString());
78
79 // Initialize member variables.
80 m_bInitialized = false;
81 m_StuckDetector = statemachine::TimeIntervalBasedStuckDetector(constants::STUCK_CHECK_ATTEMPTS,
82 constants::STUCK_CHECK_INTERVAL,
83 constants::STUCK_CHECK_VEL_THRESH,
84 constants::STUCK_CHECK_ROT_THRESH);
85 m_pRoverPathPlot = std::make_unique<logging::graphing::PathTracer>("NavigatingRoverPath");
86 m_pAStarPlanner = std::make_unique<pathplanners::AStar>();
87 m_pStanleyController = std::make_unique<controllers::PredictiveStanleyController>(constants::STANLEY_CROSSTRACK_CONTROL_GAIN,
88 constants::STANLEY_STEERING_ANGLE_LIMIT,
89 constants::STANLEY_DIST_TO_FRONT_AXLE,
90 constants::STANLEY_PREDICTION_HORIZON,
91 constants::STANLEY_PREDICTION_TIME_STEP);
92
93 // Start state.
94 if (!m_bInitialized)
95 {
96 Start();
97 m_bInitialized = true;
98 }
99 }
void Start() override
This method is called when the state is first started. It is used to initialize the state.
Definition NavigatingState.cpp:33
virtual std::string ToString() const
Accessor for the State private member. Returns the state as a string.
Definition State.hpp:207
State(States eState)
Construct a new State object.
Definition State.hpp:150
This class should be instantiated within another state to be used for detection of if the rover is st...
Definition StuckDetection.hpp:43
Here is the call graph for this function:

Member Function Documentation

◆ Start()

void statemachine::NavigatingState::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.

34 {
35 // Schedule the next run of the state's logic
36 LOG_INFO(logging::g_qSharedLogger, "NavigatingState: Scheduling next run of state logic.");
37
38 // Initialize member variables.
39 m_bFetchNewWaypoint = true;
40 m_vTagDetectors = {globals::g_pTagDetectionHandler->GetTagDetector(TagDetectionHandler::TagDetectors::eHeadMainCam)};
41 m_vObjectDetectors = {globals::g_pObjectDetectionHandler->GetObjectDetector(ObjectDetectionHandler::ObjectDetectors::eHeadMainCam)};
42
43 // Create rover path layers.
44 m_pRoverPathPlot->CreatePathLayer("NavPath", "--b");
45 m_pRoverPathPlot->CreatePathLayer("RoverPath", "-.r*");
46 m_pRoverPathPlot->CreatePathLayer("AStarPath", "-m");
47 m_pRoverPathPlot->CreateDotLayer("SmoothPath", "b", false);
48 m_pRoverPathPlot->CreateDotLayer("ObstaclesLocation", "o");
49 m_pRoverPathPlot->CreateDotLayer("DetectedTags", "green");
50 m_pRoverPathPlot->CreateDotLayer("DetectedObjects", "red");
51 }
std::shared_ptr< ObjectDetector > GetObjectDetector(ObjectDetectors eDetectorName)
Accessor for ObjectDetector detectors.
Definition ObjectDetectionHandler.cpp:127
std::shared_ptr< TagDetector > GetTagDetector(TagDetectors eDetectorName)
Accessor for TagDetector detectors.
Definition TagDetectionHandler.cpp:132
Here is the call graph for this function:
Here is the caller graph for this function:

◆ Exit()

void statemachine::NavigatingState::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.

62 {
63 // Clean up the state before exiting
64 LOG_INFO(logging::g_qSharedLogger, "NavigatingState: Exiting state.");
65 }
Here is the caller graph for this function:

◆ Run()

void statemachine::NavigatingState::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.

108 {
109 // Submit logger message.
110 LOG_DEBUG(logging::g_qSharedLogger, "NavigatingState: Running state-specific behavior.");
111
112 /*
113 The overall flow of this state is as follows.
114 1. Navigate to goal waypoint.
115 1. Is there a tag -> MarkerSeen
116 2. Is there an object -> ObjectSeen
117 3. Is there an obstacle -> TBD
118 4. Is the rover stuck -> Stuck
119 */
120
122 /* --- Navigate to goal waypoint --- */
124
125 // Check if we should get a new goal waypoint and that the waypoint handler has one for us.
126 if (m_bFetchNewWaypoint && globals::g_pWaypointHandler->GetWaypointCount() > 0)
127 {
128 // Trigger new waypoint event.
129 globals::g_pStateMachineHandler->HandleEvent(Event::eNewWaypoint, true);
130 return;
131 }
132
133 // Get Current rover pose.
134 geoops::RoverPose stCurrentRoverPose = globals::g_pWaypointHandler->SmartRetrieveRoverPose();
135 // Calculate distance and bearing from goal waypoint.
136 geoops::GeoMeasurement stGoalWaypointMeasurement = geoops::CalculateGeoMeasurement(stCurrentRoverPose.GetUTMCoordinate(), m_stGoalWaypoint.GetUTMCoordinate());
137 // Add the current rover pose to the path plot.
138 m_pRoverPathPlot->AddPathPoint(stCurrentRoverPose.GetUTMCoordinate(), "RoverPath");
139
140 // Only print out every so often.
141 static bool bAlreadyPrinted = false;
142 if ((std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count() % 5) == 0 && !bAlreadyPrinted)
143 {
144 // Get raw Navboard GPS position.
145 geoops::GPSCoordinate stCurrentGPSPosition = globals::g_pNavigationBoard->GetGPSData();
146 // Calculate error between pose and GPS.
147 geoops::GeoMeasurement stErrorMeasurement = geoops::CalculateGeoMeasurement(stCurrentRoverPose.GetGPSCoordinate(), stCurrentGPSPosition);
148
149 // Assemble the error metrics into a single string. We are going to include the distance and bearing to the goal waypoint and
150 // the error between the rover pose and the GPS position. The rover pose could be from VIO or GNSS fusion, or just GPS.
151 std::string szErrorMetrics =
152 "--------[ Navigating Error Report ]--------\nDistance to Goal Waypoint: " + std::to_string(stGoalWaypointMeasurement.dDistanceMeters) + " meters\n" +
153 "Bearing to Goal Waypoint: " + std::to_string(stGoalWaypointMeasurement.dStartRelativeBearing) + " degrees\n" +
154 "GPS/VIO Position Error (UTM for easy reading):\n" + std::to_string(stErrorMeasurement.dDistanceMeters) + " (distance) " +
155 std::to_string(stErrorMeasurement.dStartRelativeBearing) + " (bearing)";
156 // Submit the error metrics to the logger.
157 LOG_INFO(logging::g_qSharedLogger, "{}", szErrorMetrics);
158
159 // Set toggle.
160 bAlreadyPrinted = true;
161 }
162 else if ((std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count() % 5) != 0 && bAlreadyPrinted)
163 {
164 // Reset toggle.
165 bAlreadyPrinted = false;
166 }
167
168 // Check if we are at the goal waypoint.
169 if (stGoalWaypointMeasurement.dDistanceMeters > constants::NAVIGATING_REACHED_GOAL_RADIUS)
170 {
171 // NOTE: Optional - Uncomment the above code and comment out the below code to use stanley control to navigate to the goal waypoint.
172 // Use stanley to calculate drive move/powers.
173 // controllers::PredictiveStanleyController::DriveVector stDriveVector = m_pStanleyController->Calculate(stCurrentRoverPose);
174 // // Calculate move from goal heading and desired speed.
175 // diffdrive::DrivePowers stDriveSpeeds = globals::g_pDriveBoard->CalculateMove(stDriveVector.dVelocity,
176 // stDriveVector.dThetaHeading,
177 // stCurrentRoverPose.GetCompassHeading(),
178 // diffdrive::DifferentialControlMethod::eArcadeDrive);
179 diffdrive::DrivePowers stDriveSpeeds = globals::g_pDriveBoard->CalculateMove(constants::NAVIGATING_MOTOR_POWER,
180 stGoalWaypointMeasurement.dStartRelativeBearing,
181 stCurrentRoverPose.GetCompassHeading(),
182 diffdrive::DifferentialControlMethod::eArcadeDrive);
183 // Send drive powers over RoveComm.
184 globals::g_pDriveBoard->SendDrive(stDriveSpeeds);
185 }
186 else
187 {
188 // Stop drive.
189 globals::g_pDriveBoard->SendStop();
190
191 // Check waypoint type.
192 switch (m_stGoalWaypoint.eType)
193 {
194 // Goal waypoint is navigation.
195 case geoops::WaypointType::eNavigationWaypoint:
196 {
197 // Continuously navigate to the next waypoint if our current waypoint ID is set to -99.
198 if (globals::g_pWaypointHandler->GetWaypointCount() > 1 &&
199 m_stGoalWaypoint.nID == static_cast<int>(manifest::Autonomy::AUTONOMYWAYPOINTTYPES::CONTINUOUSNAVIGATE))
200 {
201 // Submit logger message.
202 LOG_NOTICE(logging::g_qSharedLogger, "NavigatingState: The current waypoint ID is {}. Continuing to next waypoint...", m_stGoalWaypoint.nID);
203 // Pop the next waypoint.
204 globals::g_pWaypointHandler->PopNextWaypoint();
205 // Trigger new waypoint event.
206 globals::g_pStateMachineHandler->HandleEvent(Event::eNewWaypoint, true);
207 }
208 else
209 {
210 // We are at the goal, signal event.
211 globals::g_pStateMachineHandler->HandleEvent(Event::eReachedGpsCoordinate, false);
212 }
213 return;
214 }
215 // Goal waypoint is marker.
216 case geoops::WaypointType::eTagWaypoint:
217 {
218 // We are at the goal, signal event.
219 globals::g_pStateMachineHandler->HandleEvent(Event::eReachedMarker, false);
220 return;
221 }
222 // Goal waypoint is object.
223 case geoops::WaypointType::eObjectWaypoint:
224 {
225 // We are at the goal, signal event.
226 globals::g_pStateMachineHandler->HandleEvent(Event::eReachedObject, false);
227 return;
228 }
229 // Goal waypoint is object.
230 case geoops::WaypointType::eMalletWaypoint:
231 {
232 // We are at the goal, signal event.
233 globals::g_pStateMachineHandler->HandleEvent(Event::eReachedObject, false);
234 return;
235 }
236 // Goal waypoint is object.
237 case geoops::WaypointType::eWaterBottleWaypoint:
238 {
239 // We are at the goal, signal event.
240 globals::g_pStateMachineHandler->HandleEvent(Event::eReachedObject, false);
241 return;
242 }
243 default:
244 {
245 // This waypoint type is not supported.
246 LOG_ERROR(logging::g_qSharedLogger, "NavigatingState: Unknown waypoint type!");
247 // Handle event.
248 globals::g_pStateMachineHandler->HandleEvent(Event::eAbort, true);
249 // Don't execute the rest of the state.
250 return;
251 }
252 }
253 }
254
256 /* --- Detect Tags --- */
258
259 // 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.
260 if (m_stGoalWaypoint.eType == geoops::WaypointType::eTagWaypoint && stGoalWaypointMeasurement.dDistanceMeters <= m_stGoalWaypoint.dRadius)
261 {
262 // Create instance variables.
263 tagdetectutils::ArucoTag stBestArucoTag, stBestTorchTag;
264 // Identify target marker.
265 statemachine::IdentifyTargetMarker(m_vTagDetectors, stBestArucoTag, stBestTorchTag, m_stGoalWaypoint.nID);
266 // Check if either tag type is seen.
267 if (stBestArucoTag.nID != -1 || stBestTorchTag.dConfidence != 0.0)
268 {
269 // Submit logger message.
270 LOG_INFO(logging::g_qSharedLogger, "NavigatingState: Rover has seen a target marker!");
271
272 // Check if the OpenCV tag has a good absolute position.
273 if (stBestArucoTag.nID != -1 && stBestArucoTag.stGeolocatedPosition.eType == geoops::WaypointType::eTagWaypoint)
274 {
275 // Add the tag to the path plot.
276 m_pRoverPathPlot->AddDot(stBestArucoTag.stGeolocatedPosition.GetUTMCoordinate(), "DetectedTags");
277 }
278 // Check if the torch tag has a good absolute position.
279 if (stBestTorchTag.dConfidence != 0.0 && stBestTorchTag.stGeolocatedPosition.eType == geoops::WaypointType::eTagWaypoint)
280 {
281 // Add the tag to the path plot.
282 m_pRoverPathPlot->AddDot(stBestTorchTag.stGeolocatedPosition.GetUTMCoordinate(), "DetectedTags");
283 }
284
285 // Handle state transition and save the current search pattern state.
286 globals::g_pStateMachineHandler->HandleEvent(Event::eMarkerSeen, true);
287 // Don't execute the rest of the state.
288 return;
289 }
290 }
291
293 /* --- Detect Objects --- */
295
296 // 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.
297 if ((m_stGoalWaypoint.eType == geoops::WaypointType::eObjectWaypoint || m_stGoalWaypoint.eType == geoops::WaypointType::eMalletWaypoint ||
298 m_stGoalWaypoint.eType == geoops::WaypointType::eWaterBottleWaypoint) &&
299 stGoalWaypointMeasurement.dDistanceMeters <= m_stGoalWaypoint.dRadius)
300 {
301 // Create instance variables.
302 objectdetectutils::Object stBestTorchObject;
303 // Identify target object.
304 statemachine::IdentifyTargetObject(m_vObjectDetectors, stBestTorchObject, m_stGoalWaypoint.eType);
305 // Check if either tag type is seen.
306 if (stBestTorchObject.dConfidence != 0.0)
307 {
308 // Submit logger message.
309 LOG_NOTICE(logging::g_qSharedLogger, "SearchPatternState: Rover has seen a target object!");
310
311 // Check if the torch tag has a good absolute position.
312 if (stBestTorchObject.dConfidence != 0.0 && stBestTorchObject.stGeolocatedPosition.eType == geoops::WaypointType::eObjectWaypoint)
313 {
314 // Add the tag to the path plot.
315 m_pRoverPathPlot->AddDot(stBestTorchObject.stGeolocatedPosition.GetUTMCoordinate(), "DetectedObjects");
316 }
317
318 // Handle state transition and save the current search pattern state.
319 globals::g_pStateMachineHandler->HandleEvent(Event::eObjectSeen, true);
320 // Don't execute the rest of the state.
321 return;
322 }
323 }
324
326 /* --- Detect Obstacles --- */
328
329 // TODO: Add obstacle detection to Navigating state
330
332 /* --- Check if the rover is stuck --- */
334
335 // Check if stuck.
336 if (constants::NAVIGATING_ENABLE_STUCK_DETECT &&
337 m_StuckDetector.CheckIfStuck(globals::g_pWaypointHandler->SmartRetrieveVelocity(), globals::g_pWaypointHandler->SmartRetrieveAngularVelocity()))
338 {
339 // Submit logger message.
340 LOG_NOTICE(logging::g_qSharedLogger, "NavigatingState: Rover has become stuck!");
341 // Handle state transition and save the current search pattern state.
342 globals::g_pStateMachineHandler->HandleEvent(Event::eStuck, true);
343 // Don't execute the rest of the state.
344 return;
345 }
346 }
void SendDrive(const diffdrive::DrivePowers &stDrivePowers)
Sets the left and right drive powers of the drive board.
Definition DriveBoard.cpp:117
void SendStop()
Stop the drivetrain of the Rover.
Definition DriveBoard.cpp:163
diffdrive::DrivePowers CalculateMove(const double dGoalSpeed, const double dGoalHeading, const double dActualHeading, const diffdrive::DifferentialControlMethod eKinematicsMethod=diffdrive::DifferentialControlMethod::eArcadeDrive, const bool bAlwaysProgressForward=false)
This method determines drive powers to make the Rover drive towards a given heading at a given speed.
Definition DriveBoard.cpp:87
geoops::GPSCoordinate GetGPSData()
Accessor for most recent GPS data received from NavBoard.
Definition NavigationBoard.cpp:78
void HandleEvent(statemachine::Event eEvent, const bool bSaveCurrentState=false)
This method Handles Events that are passed to the State Machine Handler. It will check the current st...
Definition StateMachineHandler.cpp:350
geoops::RoverPose SmartRetrieveRoverPose(bool bVIOHeading=true, bool bVIOTracking=false)
Retrieve the rover's current position and heading. Automatically picks between getting the position/h...
Definition WaypointHandler.cpp:742
geoops::Waypoint PopNextWaypoint()
Removes and returns the next waypoint at the front of the list.
Definition WaypointHandler.cpp:500
bool CheckIfStuck(double dCurrentVelocity, double dCurrentAngularVelocity)
Checks if the rover meets stuck criteria based in the given parameters.
Definition StuckDetection.hpp:105
GeoMeasurement CalculateGeoMeasurement(const GPSCoordinate &stCoord1, const GPSCoordinate &stCoord2)
The shortest path between two points on an ellipsoid at (lat1, lon1) and (lat2, lon2) is called the g...
Definition GeospatialOperations.hpp:522
int IdentifyTargetMarker(const std::vector< std::shared_ptr< TagDetector > > &vTagDetectors, tagdetectutils::ArucoTag &stArucoTarget, tagdetectutils::ArucoTag &stTorchTarget, const int nTargetTagID=static_cast< int >(manifest::Autonomy::AUTONOMYWAYPOINTTYPES::ANY))
Identify a target marker in the rover's vision, using OpenCV detection.
Definition TagDetectionChecker.hpp:91
int IdentifyTargetObject(const std::vector< std::shared_ptr< ObjectDetector > > &vObjectDetectors, objectdetectutils::Object &stObjectTarget, const geoops::WaypointType &eDesiredDetectionType=geoops::WaypointType::eUNKNOWN)
Identify a target object in the rover's vision, using Torch detection.
Definition ObjectDetectionChecker.hpp:90
This struct is used to store the left and right drive powers for the robot. Storing these values in a...
Definition DifferentialDrive.hpp:73
This struct stores/contains information about a GPS data.
Definition GeospatialOperations.hpp:99
This struct is used to store the distance, arc length, and relative bearing for a calculated geodesic...
Definition GeospatialOperations.hpp:82
This struct is used by the WaypointHandler to provide an easy way to store all pose data about the ro...
Definition GeospatialOperations.hpp:677
const geoops::GPSCoordinate & GetGPSCoordinate() const
Accessor for the geoops::GPSCoordinate member variable.
Definition GeospatialOperations.hpp:725
double GetCompassHeading() const
Accessor for the Compass Heading private member.
Definition GeospatialOperations.hpp:756
const geoops::UTMCoordinate & GetUTMCoordinate() const
Accessor for the geoops::UTMCoordinate member variable.
Definition GeospatialOperations.hpp:736
const geoops::UTMCoordinate & GetUTMCoordinate() const
Accessor for the geoops::UTMCoordinate member variable.
Definition GeospatialOperations.hpp:477
Represents a single detected object. Combines attributes from TorchObject and TensorflowObject struct...
Definition ObjectDetectionUtility.hpp:73
Represents a single ArUco tag. Combines attributes from TorchTag, TensorflowTag, and the original Aru...
Definition TagDetectionUtilty.hpp:59
Here is the call graph for this function:

◆ TriggerEvent()

States statemachine::NavigatingState::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.

358 {
359 // Create instance variables.
360 States eNextState = States::eNavigating;
361 bool bCompleteStateExit = true;
362
363 switch (eEvent)
364 {
365 case Event::eNoWaypoint:
366 {
367 // Submit logger message.
368 LOG_INFO(logging::g_qSharedLogger, "NavigatingState: Handling No Waypoint event.");
369 // Change state.
370 eNextState = States::eIdle;
371 break;
372 }
373 case Event::eReachedGpsCoordinate:
374 {
375 // Submit logger message.
376 LOG_INFO(logging::g_qSharedLogger, "NavigatingState: Handling Reached GPS Coordinate event.");
377 // Check constants to see if we should go into verifying position or just trigger reached marker.
378 if (constants::NAVIGATING_VERIFY_POSITION)
379 {
380 // Send multimedia command to update state display.
381 globals::g_pMultimediaBoard->SendLightingState(MultimediaBoard::MultimediaBoardLightingState::eAutonomy);
382 // Change state.
383 eNextState = States::eVerifyingPosition;
384 }
385 else
386 {
387 // Send multimedia command to update state display.
388 globals::g_pMultimediaBoard->SendLightingState(MultimediaBoard::MultimediaBoardLightingState::eReachedGoal);
389 // Pop the next waypoint.
390 globals::g_pWaypointHandler->PopNextWaypoint();
391 // Change state.
392 eNextState = States::eIdle;
393 }
394 break;
395 }
396 case Event::eReachedMarker:
397 {
398 // Submit logger message.
399 LOG_INFO(logging::g_qSharedLogger, "NavigatingState: Handling Reached Marker Waypoint event.");
400 // Send multimedia command to update state display.
401 globals::g_pMultimediaBoard->SendLightingState(MultimediaBoard::MultimediaBoardLightingState::eAutonomy);
402 // Change state.
403 eNextState = States::eSearchPattern;
404 break;
405 }
406 case Event::eReachedObject:
407 {
408 // Submit logger message.
409 LOG_INFO(logging::g_qSharedLogger, "NavigatingState: Handling Reached Object Waypoint event.");
410 // Send multimedia command to update state display.
411 globals::g_pMultimediaBoard->SendLightingState(MultimediaBoard::MultimediaBoardLightingState::eAutonomy);
412 // Change state.
413 eNextState = States::eSearchPattern;
414 break;
415 }
416 case Event::eNewWaypoint:
417 {
418 // Check if the next goal waypoint equals the current one.
419 if (m_stGoalWaypoint == globals::g_pWaypointHandler->PeekNextWaypoint())
420 {
421 // Submit logger message.
422 LOG_INFO(logging::g_qSharedLogger, "NavigatingState: Reusing current Waypoint.");
423 }
424 else
425 {
426 // Submit logger message.
427 LOG_INFO(logging::g_qSharedLogger, "NavigatingState: Handling New Waypoint event.");
428 // Get and store new goal waypoint.
429 m_stGoalWaypoint = globals::g_pWaypointHandler->PeekNextWaypoint();
430 // Clear the old path plot and add the new path.
431 m_pRoverPathPlot->ClearLayer("NavPath");
432 // Add starting point and goal point to path plot.
433 m_pRoverPathPlot->AddPathPoint(globals::g_pWaypointHandler->SmartRetrieveRoverPose().GetUTMCoordinate(), "NavPath", 0);
434 m_pRoverPathPlot->AddPathPoint(m_stGoalWaypoint, "NavPath", 0);
435
436 // Get all obstacles from the obstacle handler.
437 std::vector<geoops::Waypoint> vObstacles = globals::g_pWaypointHandler->GetAllObstacles();
438 // Add obstacles to the A* planner.
439 m_pAStarPlanner->UpsertObstacleData(vObstacles);
440 // Set A* planner start and goal.
441 m_vPathCoordinates =
442 m_pAStarPlanner->PlanAvoidancePath(globals::g_pWaypointHandler->SmartRetrieveRoverPose().GetUTMCoordinate(), m_stGoalWaypoint.GetUTMCoordinate());
443 // Set the path of the stanley controller.
444 m_pStanleyController->SetReferencePath(m_vPathCoordinates);
445 // Get the smoothed path for plotting.
446 std::vector<geoops::Waypoint> vSmoothedPath = m_pStanleyController->GetReferencePath();
447 // Update our plot with the new path.
448 m_pRoverPathPlot->ClearLayer("AStarPath");
449 m_pRoverPathPlot->AddPathPoints(m_vPathCoordinates, "AStarPath", 0);
450 m_pRoverPathPlot->AddDots(vSmoothedPath, "SmoothPath", 0);
451 m_pRoverPathPlot->AddDots(vObstacles, "ObstaclesLocation", 0);
452 }
453
454 // Send multimedia command to update state display.
455 globals::g_pMultimediaBoard->SendLightingState(MultimediaBoard::MultimediaBoardLightingState::eAutonomy);
456 // Set toggle.
457 m_bFetchNewWaypoint = false;
458 break;
459 }
460 case Event::eStart:
461 {
462 // Submit logger message.
463 LOG_INFO(logging::g_qSharedLogger, "NavigatingState: Handling Start event.");
464 // Send multimedia command to update state display.
465 globals::g_pMultimediaBoard->SendLightingState(MultimediaBoard::MultimediaBoardLightingState::eAutonomy);
466 break;
467 }
468 case Event::eAbort:
469 {
470 // Submit logger message.
471 LOG_INFO(logging::g_qSharedLogger, "NavigatingState: Handling Abort event.");
472 // Stop drive.
473 globals::g_pDriveBoard->SendStop();
474 // Send multimedia command to update state display.
475 globals::g_pMultimediaBoard->SendLightingState(MultimediaBoard::MultimediaBoardLightingState::eOff);
476 // Set toggle.
477 m_bFetchNewWaypoint = true;
478 // Change states.
479 eNextState = States::eIdle;
480 break;
481 }
482 case Event::eMarkerSeen:
483 {
484 // Submit logger message.
485 LOG_INFO(logging::g_qSharedLogger, "NavigatingState: Handling MarkerSeen event.");
486 // Change states.
487 eNextState = States::eApproachingMarker;
488 break;
489 }
490 case Event::eObjectSeen:
491 {
492 // Submit logger message.
493 LOG_INFO(logging::g_qSharedLogger, "NavigatingState: Handling ObjectSeen event.");
494 // Change states.
495 eNextState = States::eApproachingObject;
496 break;
497 }
498 case Event::eObstacleAvoidance:
499 {
500 LOG_INFO(logging::g_qSharedLogger, "NavigatingState: Handling Obstacle Avoidance event.");
501 eNextState = States::eAvoidance;
502 break;
503 }
504 case Event::eReverse:
505 {
506 LOG_INFO(logging::g_qSharedLogger, "NavigatingState: Handling Reverse event.");
507 eNextState = States::eReversing;
508 break;
509 }
510 case Event::eStuck:
511 {
512 LOG_INFO(logging::g_qSharedLogger, "NavigatingState: Handling Stuck event.");
513 eNextState = States::eStuck;
514 break;
515 }
516 default:
517 {
518 LOG_WARNING(logging::g_qSharedLogger, "NavigatingState: Handling unknown event.");
519 eNextState = States::eIdle;
520 break;
521 }
522 }
523
524 if (eNextState != States::eNavigating)
525 {
526 LOG_INFO(logging::g_qSharedLogger, "NavigatingState: Transitioning to {} State.", StateToString(eNextState));
527
528 // Exit the current state
529 if (bCompleteStateExit)
530 {
531 Exit();
532 }
533 }
534
535 return eNextState;
536 }
void SendLightingState(MultimediaBoardLightingState eState)
Sends a predetermined color pattern to board.
Definition MultimediaBoard.cpp:55
const geoops::Waypoint PeekNextWaypoint()
Returns an immutable reference to the geoops::Waypoint struct at the front of the list without removi...
Definition WaypointHandler.cpp:540
const std::vector< geoops::Waypoint > GetAllObstacles()
Accessor for the full list of current obstacle stored in the WaypointHandler.
Definition WaypointHandler.cpp:673
void Exit() override
This method is called when the state is exited. It is used to clean up the state.
Definition NavigatingState.cpp:61
std::string StateToString(States eState)
Converts a state object to a string.
Definition State.hpp:89
States
The states that the state machine can be in.
Definition State.hpp:31
Here is the call graph for this function:

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