Run the state machine. Returns the next state.
108 {
109
110 LOG_DEBUG(logging::g_qSharedLogger, "NavigatingState: Running state-specific behavior.");
111
112
113
114
115
116
117
118
119
120
122
124
125
126 if (m_bFetchNewWaypoint && globals::g_pWaypointHandler->GetWaypointCount() > 0)
127 {
128
129 globals::g_pStateMachineHandler->
HandleEvent(Event::eNewWaypoint,
true);
130 return;
131 }
132
133
135
137
138 m_pRoverPathPlot->AddPathPoint(stCurrentRoverPose.
GetUTMCoordinate(),
"RoverPath");
139
140
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
146
148
149
150
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
157 LOG_INFO(logging::g_qSharedLogger, "{}", szErrorMetrics);
158
159
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
165 bAlreadyPrinted = false;
166 }
167
168
169 if (stGoalWaypointMeasurement.dDistanceMeters > constants::NAVIGATING_REACHED_GOAL_RADIUS)
170 {
171
172
173
174
175
176
177
178
180 stGoalWaypointMeasurement.dStartRelativeBearing,
182 diffdrive::DifferentialControlMethod::eArcadeDrive);
183
184 globals::g_pDriveBoard->
SendDrive(stDriveSpeeds);
185 }
186 else
187 {
188
190
191
192 switch (m_stGoalWaypoint.eType)
193 {
194
195 case geoops::WaypointType::eNavigationWaypoint:
196 {
197
198 if (globals::g_pWaypointHandler->GetWaypointCount() > 1 &&
199 m_stGoalWaypoint.nID == static_cast<int>(manifest::Autonomy::AUTONOMYWAYPOINTTYPES::CONTINUOUSNAVIGATE))
200 {
201
202 LOG_NOTICE(logging::g_qSharedLogger, "NavigatingState: The current waypoint ID is {}. Continuing to next waypoint...", m_stGoalWaypoint.nID);
203
205
206 globals::g_pStateMachineHandler->
HandleEvent(Event::eNewWaypoint,
true);
207 }
208 else
209 {
210
211 globals::g_pStateMachineHandler->
HandleEvent(Event::eReachedGpsCoordinate,
false);
212 }
213 return;
214 }
215
216 case geoops::WaypointType::eTagWaypoint:
217 {
218
219 globals::g_pStateMachineHandler->
HandleEvent(Event::eReachedMarker,
false);
220 return;
221 }
222
223 case geoops::WaypointType::eObjectWaypoint:
224 {
225
226 globals::g_pStateMachineHandler->
HandleEvent(Event::eReachedObject,
false);
227 return;
228 }
229
230 case geoops::WaypointType::eMalletWaypoint:
231 {
232
233 globals::g_pStateMachineHandler->
HandleEvent(Event::eReachedObject,
false);
234 return;
235 }
236
237 case geoops::WaypointType::eWaterBottleWaypoint:
238 {
239
240 globals::g_pStateMachineHandler->
HandleEvent(Event::eReachedObject,
false);
241 return;
242 }
243 default:
244 {
245
246 LOG_ERROR(logging::g_qSharedLogger, "NavigatingState: Unknown waypoint type!");
247
248 globals::g_pStateMachineHandler->
HandleEvent(Event::eAbort,
true);
249
250 return;
251 }
252 }
253 }
254
256
258
259
260 if (m_stGoalWaypoint.eType == geoops::WaypointType::eTagWaypoint && stGoalWaypointMeasurement.dDistanceMeters <= m_stGoalWaypoint.dRadius)
261 {
262
264
266
267 if (stBestArucoTag.nID != -1 || stBestTorchTag.dConfidence != 0.0)
268 {
269
270 LOG_INFO(logging::g_qSharedLogger, "NavigatingState: Rover has seen a target marker!");
271
272
273 if (stBestArucoTag.nID != -1 && stBestArucoTag.stGeolocatedPosition.eType == geoops::WaypointType::eTagWaypoint)
274 {
275
276 m_pRoverPathPlot->AddDot(stBestArucoTag.stGeolocatedPosition.
GetUTMCoordinate(),
"DetectedTags");
277 }
278
279 if (stBestTorchTag.dConfidence != 0.0 && stBestTorchTag.stGeolocatedPosition.eType == geoops::WaypointType::eTagWaypoint)
280 {
281
282 m_pRoverPathPlot->AddDot(stBestTorchTag.stGeolocatedPosition.
GetUTMCoordinate(),
"DetectedTags");
283 }
284
285
286 globals::g_pStateMachineHandler->
HandleEvent(Event::eMarkerSeen,
true);
287
288 return;
289 }
290 }
291
293
295
296
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
303
305
306 if (stBestTorchObject.dConfidence != 0.0)
307 {
308
309 LOG_NOTICE(logging::g_qSharedLogger, "SearchPatternState: Rover has seen a target object!");
310
311
312 if (stBestTorchObject.dConfidence != 0.0 && stBestTorchObject.stGeolocatedPosition.eType == geoops::WaypointType::eObjectWaypoint)
313 {
314
315 m_pRoverPathPlot->AddDot(stBestTorchObject.stGeolocatedPosition.
GetUTMCoordinate(),
"DetectedObjects");
316 }
317
318
319 globals::g_pStateMachineHandler->
HandleEvent(Event::eObjectSeen,
true);
320
321 return;
322 }
323 }
324
326
328
329
330
332
334
335
336 if (constants::NAVIGATING_ENABLE_STUCK_DETECT &&
337 m_StuckDetector.
CheckIfStuck(globals::g_pWaypointHandler->SmartRetrieveVelocity(), globals::g_pWaypointHandler->SmartRetrieveAngularVelocity()))
338 {
339
340 LOG_NOTICE(logging::g_qSharedLogger, "NavigatingState: Rover has become stuck!");
341
342 globals::g_pStateMachineHandler->
HandleEvent(Event::eStuck,
true);
343
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