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
SearchPattern.hpp
Go to the documentation of this file.
1
12#ifndef SEARCH_PATTERN_HPP
13#define SEARCH_PATTERN_HPP
14
15#include "../util/GeospatialOperations.hpp"
16
18#include <cmath>
19#include <vector>
20
22
23
31{
32
49 inline std::vector<geoops::Waypoint> CalculateSpiralPatternWaypoints(const geoops::Waypoint& stStartingPoint,
50 const double dAngularStepDegrees = 57,
51 const double dMaxRadius = 25,
52 const double dStartingHeadingDegrees = 0,
53 const double dStartSpacing = 1)
54 {
55 // Define variables.
56 std::vector<geoops::Waypoint> vWaypoints;
57 double dAngularStepRadians = dAngularStepDegrees * M_PI / 180;
58 double dAngleRadians = (dStartingHeadingDegrees + 90) * M_PI / 180;
59 double dCurrentSpacingWindUp = 0.0;
60 double dStartingX = stStartingPoint.GetUTMCoordinate().dEasting;
61 double dStartingY = stStartingPoint.GetUTMCoordinate().dNorthing;
62 double dCurrentRadius = 0.0;
63
64 // Check if the MaxRadius is less than 1 meter. If so return an empty vector.
65 if (dMaxRadius < 1)
66 {
67 // Submit logger message.
68 LOG_WARNING(logging::g_qSharedLogger, "MaxRadius is less than 1 meter. Cannot create spiral pattern.");
69 return vWaypoints;
70 }
71
72 // Calculate each waypoint. Stop when the radius exceeds the maximum.
73 while (dCurrentRadius <= dMaxRadius)
74 {
75 // Get X and Y positions for the current point.
76 double dCurrentX = dStartingX + dCurrentSpacingWindUp * cos(dAngleRadians);
77 double dCurrentY = dStartingY + dCurrentSpacingWindUp * sin(dAngleRadians);
78
79 // Add the current waypoint to the final vector.
80 geoops::UTMCoordinate stCurrentCoordinate = stStartingPoint.GetUTMCoordinate();
81 stCurrentCoordinate.dEasting = dCurrentX;
82 stCurrentCoordinate.dNorthing = dCurrentY;
83 geoops::Waypoint stCurrentWaypoint(stCurrentCoordinate, geoops::WaypointType::eNavigationWaypoint);
84 vWaypoints.push_back(stCurrentWaypoint);
85
86 // Increment angle and radius for the next waypoint.
87 dAngleRadians += dAngularStepRadians;
88 dCurrentSpacingWindUp += dStartSpacing;
89
90 // Calculate the current distance from the starting point. This is our radius.
91 dCurrentRadius = geoops::CalculateGeoMeasurement(stStartingPoint.GetUTMCoordinate(), stCurrentCoordinate).dDistanceMeters;
92 }
93
94 // Write the search pattern points to the logger, just store the GPS lat/long.
95 std::string szSearchPatternPoints = "Search Pattern Points (Spiral): ";
96 for (geoops::Waypoint& stWaypoint : vWaypoints)
97 {
98 szSearchPatternPoints +=
99 "(" + std::to_string(stWaypoint.GetGPSCoordinate().dLatitude) + ", " + std::to_string(stWaypoint.GetGPSCoordinate().dLongitude) + "), ";
100 }
101 // Submit logger message.
102 LOG_DEBUG(logging::g_qSharedLogger, "{}", szSearchPatternPoints);
103
104 return vWaypoints;
105 }
106
107
124 inline std::vector<geoops::Waypoint> CalculateZigZagPatternWaypoints(const geoops::Waypoint& stCenterPoint,
125 const double dWidth = 20.0,
126 const double dHeight = 20.0,
127 const double dSpacing = 1.0,
128 const bool bVertical = true)
129 {
130 // Create instance variables.
131 std::vector<geoops::Waypoint> vWaypoints;
132 double dStartingX = stCenterPoint.GetUTMCoordinate().dEasting - (dWidth / 2);
133 double dStartingY = stCenterPoint.GetUTMCoordinate().dNorthing - (dHeight / 2);
134 double dCurrentX = dStartingX;
135 double dCurrentY = dStartingY;
136 bool bZigNotZag = true;
137 double bCalcSpacing = dSpacing;
138
139 // Check if the width or height is less than 1 meter. If so return an empty vector.
140 if (dWidth < 1 || dHeight < 1 || dSpacing < 1)
141 {
142 // Submit logger message.
143 LOG_WARNING(logging::g_qSharedLogger, "Width or height or spacing is less than 1 meter. Cannot create zigzag pattern.");
144 return vWaypoints;
145 }
146
147 // Limit spacing to the width or height.
148 if (bCalcSpacing > dWidth / 2.0)
149 {
150 // Submit logger message.
151 LOG_WARNING(logging::g_qSharedLogger, "Spacing is greater than width. Setting spacing to width / 2.");
152 // Set spacing to half the width.
153 bCalcSpacing = dWidth / 2.0 - 1.0;
154 }
155 if (bCalcSpacing > dHeight / 2.0)
156 {
157 // Submit logger message.
158 LOG_WARNING(logging::g_qSharedLogger, "Spacing is greater than height. Setting spacing to height / 2.");
159 // Set spacing to half the height.
160 bCalcSpacing = dHeight / 2.0 - 1.0;
161 }
162
163 // Loop until covered entire space of width and height.
164 while ((bVertical && dCurrentY <= dStartingY + dHeight) || (!bVertical && dCurrentX <= dStartingX + dWidth))
165 {
166 // Check if pattern should be vertical or horizontal.
167 if (bVertical)
168 {
169 // Check step direction.
170 if (bZigNotZag)
171 {
172 // Zig.
173 dCurrentX = dStartingX + bCalcSpacing;
174 }
175 else
176 {
177 // Zag.
178 dCurrentX = dStartingX - bCalcSpacing;
179 }
180 }
181 else
182 {
183 // Check step direction.
184 if (bZigNotZag)
185 {
186 // Zig.
187 dCurrentY = dStartingY + bCalcSpacing;
188 }
189 else
190 {
191 // Zag.
192 dCurrentY = dStartingY - bCalcSpacing;
193 }
194 }
195
196 // Loop and add points along line until we reached the limit or width or height.
197 while ((bZigNotZag && bVertical && dCurrentX <= dStartingX + dWidth) || (!bZigNotZag && bVertical && dCurrentX >= dStartingX) ||
198 (bZigNotZag && !bVertical && dCurrentY <= dStartingY + dHeight) || (!bZigNotZag && !bVertical && dCurrentY >= dStartingY))
199 {
200 // Construct UTMCoordinate.
201 geoops::UTMCoordinate stCurrentCoordinate = stCenterPoint.GetUTMCoordinate();
202 stCurrentCoordinate.dEasting = dCurrentX;
203 stCurrentCoordinate.dNorthing = dCurrentY;
204 geoops::Waypoint stCurrentWaypoint(stCurrentCoordinate, geoops::WaypointType::eNavigationWaypoint);
205 // Add current waypoint to final path.
206 vWaypoints.push_back(stCurrentWaypoint);
207
208 // Move to the next point based on spacing and direction.
209 if (bVertical)
210 {
211 // Increment current position.
212 dCurrentX += bZigNotZag ? bCalcSpacing : -bCalcSpacing;
213 }
214 else
215 {
216 // Increment current position.
217 dCurrentY += bZigNotZag ? bCalcSpacing : -bCalcSpacing;
218 }
219 }
220
221 // Now shift the opposite coordinate forward.
222 if (bVertical)
223 {
224 dCurrentY += bCalcSpacing;
225 }
226 else
227 {
228 dCurrentX += bCalcSpacing;
229 }
230
231 // Toggle zigzag direction.
232 bZigNotZag = !bZigNotZag;
233 }
234
235 // The path now contains the zigzag pattern with points spaced at the specified distance. This means that there
236 // are many points potentially very close to each other. This is not ideal for navigation, so we will filter out
237 // points that are too close to each other, by calculating their GeoMeasurement and testing if the distance is greater
238 // then the CalcSpacing. If the CalcSpacing is greater a certain threshold, we will remove the point since it jumps to the other side.
239 // This will reduce the number of points in the zigzag pattern, making it more efficient for navigation.
240
241 // Create a vector to store the filtered waypoints.
242 std::vector<geoops::Waypoint> vFilterWaypoints;
243 // Always store the first point.
244 vFilterWaypoints.push_back(vWaypoints[0]);
245 // Loop through the waypoints and remove any that are too close to each other.
246 for (size_t i = 0; i < vWaypoints.size() - 1; ++i)
247 {
248 // Calculate the GeoMeasurement between the current waypoint and the next waypoint.
249 geoops::GeoMeasurement stGeoMeasurement = geoops::CalculateGeoMeasurement(vWaypoints[i].GetGPSCoordinate(), vWaypoints[i + 1].GetGPSCoordinate());
250 if (stGeoMeasurement.dDistanceMeters > bCalcSpacing * 1.5)
251 {
252 // Add the points to the filtered waypoints.
253 vFilterWaypoints.push_back(vWaypoints[i]);
254 vFilterWaypoints.push_back(vWaypoints[i + 1]);
255 }
256 }
257
258 // Write the search pattern points to the logger, just store the GPS lat/long.
259 std::string szSearchPatternPoints = "Search Pattern Points (Spiral): ";
260 for (geoops::Waypoint& stWaypoint : vWaypoints)
261 {
262 szSearchPatternPoints +=
263 "(" + std::to_string(stWaypoint.GetGPSCoordinate().dLatitude) + ", " + std::to_string(stWaypoint.GetGPSCoordinate().dLongitude) + "), ";
264 }
265 // Submit logger message.
266 LOG_DEBUG(logging::g_qSharedLogger, "{}", szSearchPatternPoints);
267
268 // Return the final path.
269 return vFilterWaypoints;
270 }
271
272
286 inline std::vector<geoops::Waypoint> CalculateSnakeSearchPattern(const geoops::Waypoint& stStartCoord,
287 const double dWidth = 20.0,
288 const double dHeight = 20.0,
289 const double dSpacing = 1.0,
290 const int nNumberOfSlithers = 1.0,
291 const bool bVertical = true)
292 {
293 // Create instance variables.
294 std::vector<geoops::Waypoint> vWaypoints;
295 geoops::UTMCoordinate stStartUTM = stStartCoord.GetUTMCoordinate();
296 double dStartingX = (bVertical) ? stStartUTM.dEasting : stStartUTM.dEasting - (dWidth / 2);
297 double dStartingY = (bVertical) ? stStartUTM.dNorthing - (dHeight / 2) : stStartUTM.dNorthing;
298
299 // Check if the dimensions are valid.
300 if (dWidth < 1.0 || dHeight < 1.0 || dSpacing < 0.1 || nNumberOfSlithers < 1)
301 {
302 // Submit logger message.
303 LOG_WARNING(logging::g_qSharedLogger,
304 "Invalid parameters for snake pattern: width={}, height={}, spacing={}, number of slithers={}",
305 dWidth,
306 dHeight,
307 dSpacing,
308 nNumberOfSlithers);
309 return vWaypoints;
310 }
311
312 // Calculate the number of points based on path length and spacing and the number of slithers.
313 // For a sine wave, we need enough points to make it smooth.
314 int nPoints = static_cast<int>(dWidth + dHeight / dSpacing);
315
316 // Generate the sine wave pattern.
317 for (int nIter = 0; nIter < nPoints; ++nIter)
318 {
319 double dTime = static_cast<double>(nIter) / (nPoints - 1);
320 double dCurrentX, dCurrentY;
321
322 if (bVertical)
323 {
324 // Vertical primary movement: sinusoidal variation in x.
325 dCurrentY = dStartingY + dTime * dWidth;
326 // Sine wave with multiple periods.
327 dCurrentX = dStartingX + (dHeight / 2.0) * std::cos(2.0 * nNumberOfSlithers * M_PI * dTime);
328 }
329 else
330 {
331 // Horizontal primary movement: sinusoidal variation in y.
332 dCurrentX = dStartingX + dTime * dWidth;
333 // Sine wave with multiple periods.
334 dCurrentY = dStartingY + (dHeight / 2.0) * std::cos(2.0 * nNumberOfSlithers * M_PI * dTime);
335 }
336
337 // Create waypoint at current position.
338 geoops::UTMCoordinate stCurrentUTM = stStartUTM;
339 stCurrentUTM.dEasting = dCurrentX;
340 stCurrentUTM.dNorthing = dCurrentY;
341 geoops::Waypoint stCurrentWaypoint(stCurrentUTM, geoops::WaypointType::eNavigationWaypoint);
342 vWaypoints.push_back(stCurrentWaypoint);
343 }
344
345 // Log the waypoints.
346 std::string szSearchPatternPoints = "Search Pattern Points (Snake): ";
347 for (const geoops::Waypoint& stWaypoint : vWaypoints)
348 {
349 szSearchPatternPoints +=
350 "(" + std::to_string(stWaypoint.GetGPSCoordinate().dLatitude) + ", " + std::to_string(stWaypoint.GetGPSCoordinate().dLongitude) + "), ";
351 }
352 LOG_DEBUG(logging::g_qSharedLogger, "{}", szSearchPatternPoints);
353
354 return vWaypoints;
355 }
356
357} // namespace searchpattern
358#endif
__device__ __forceinline__ float1 cos(const uchar1 &a)
__device__ __forceinline__ float4 sin(const uchar4 &a)
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
Namespace containing algorithms related to calculating drive powers, odometry, trajectories,...
Definition SearchPattern.hpp:31
std::vector< geoops::Waypoint > CalculateSpiralPatternWaypoints(const geoops::Waypoint &stStartingPoint, const double dAngularStepDegrees=57, const double dMaxRadius=25, const double dStartingHeadingDegrees=0, const double dStartSpacing=1)
Perform a spiral search pattern starting from a given point.
Definition SearchPattern.hpp:49
std::vector< geoops::Waypoint > CalculateSnakeSearchPattern(const geoops::Waypoint &stStartCoord, const double dWidth=20.0, const double dHeight=20.0, const double dSpacing=1.0, const int nNumberOfSlithers=1.0, const bool bVertical=true)
Calculate waypoints for a snake search pattern.
Definition SearchPattern.hpp:286
std::vector< geoops::Waypoint > CalculateZigZagPatternWaypoints(const geoops::Waypoint &stCenterPoint, const double dWidth=20.0, const double dHeight=20.0, const double dSpacing=1.0, const bool bVertical=true)
Calculate waypoints for a zigzag pattern. This function generates waypoints for a zigzag pattern star...
Definition SearchPattern.hpp:124
This struct is used to store the distance, arc length, and relative bearing for a calculated geodesic...
Definition GeospatialOperations.hpp:82
This struct stores/contains information about a UTM coordinate.
Definition GeospatialOperations.hpp:195
This struct is used by the WaypointHandler class to store location, size, and type information about ...
Definition GeospatialOperations.hpp:392
const geoops::UTMCoordinate & GetUTMCoordinate() const
Accessor for the geoops::UTMCoordinate member variable.
Definition GeospatialOperations.hpp:477