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
PathTracer.hpp
Go to the documentation of this file.
1
12#ifndef PATH_TRACER_HPP
13#define PATH_TRACER_HPP
14
15#include "../GeospatialOperations.hpp"
16
18#include <chrono>
19#include <cmath>
20#include <matplot/matplot.h>
21
23
24
32namespace logging
33{
34
43 namespace graphing
44 {
45
54 {
55 public:
56
65 PathTracer(const std::string& szPlotTitle = "Graph", bool bEnable3D = false)
66 {
67 // Initialize member variables.
68 m_mtRoverPathPlot = matplot::figure(true);
69 m_mtRoverPathAxes = m_mtRoverPathPlot->current_axes();
70 m_szPlotTitle = szPlotTitle;
71 m_bEnable3D = bEnable3D;
72
73 // Check if a file with the same title name already exists. If so then append a number to the end of the file name and recheck.
74 std::string szPlotSavePath = logging::g_szLoggingOutputPath + "/path_plots/" + m_szPlotTitle;
75 m_szCurrentPlotSavePath = logging::g_szLoggingOutputPath + "/path_plots/CurrentPlot.png";
76 int nFileNum = 0;
77 while (std::filesystem::exists(szPlotSavePath + std::to_string(nFileNum) + ".png"))
78 {
79 ++nFileNum;
80 }
81 // Add the file number to the file name.
82 szPlotSavePath = szPlotSavePath + std::to_string(nFileNum);
83 // Check if the final directory exists. If not then create it.
84 if (!std::filesystem::exists(logging::g_szLoggingOutputPath + "/path_plots"))
85 {
86 std::filesystem::create_directory(logging::g_szLoggingOutputPath + "/path_plots");
87 }
88
89 // Configure the matplotplusplus gnuplot backend to not display the plot, instead save it to a file.
90 m_mtRoverPathPlot->backend()->output(szPlotSavePath + ".png");
91
92 // Make sure plot title is not empty.
93 if (m_szPlotTitle.empty())
94 {
95 // Submit logger message.
96 LOG_WARNING(logging::g_qSharedLogger, "Plot title is empty. Setting title to default.");
97 m_szPlotTitle = "RoverPath";
98 }
99
100 // Configure plot.
101 m_mtRoverPathPlot->title(m_szPlotTitle);
102 m_mtRoverPathAxes->xlabel("Easting");
103 m_mtRoverPathAxes->ylabel("Northing");
104 m_mtRoverPathAxes->zlabel("Altitude");
105 }
106
107
115 {
116 // Nothing to do yet.
117 }
118
119
171 bool CreatePathLayer(const std::string& szLayerName, const std::string& szStyleString = "-o")
172 {
173 // Check if the layer name exists in the map.
174 if (m_umPathMap.find(szLayerName) != m_umPathMap.end())
175 {
176 // Submit logger message.
177 LOG_WARNING(logging::g_qSharedLogger, "Layer already exists. Cannot create layer.");
178 return false;
179 }
180
181 // Add the layer to the maps.
182 m_umPathMap[szLayerName] = std::vector<std::tuple<double, double, double>>();
183 m_umPathLineStyleMap[szLayerName] = szStyleString;
184 m_umLastPlotUpdateTimeMap[szLayerName] = std::chrono::system_clock::now();
185
186 return true;
187 }
188
189
210 bool CreateDotLayer(const std::string& szLayerName, const std::string& szColorString = "blue", const bool bFillMarkerFace = true)
211 {
212 // Check if the layer name exists in the map.
213 if (m_umDotMap.find(szLayerName) != m_umDotMap.end())
214 {
215 // Submit logger message.
216 LOG_WARNING(logging::g_qSharedLogger, "Layer already exists. Cannot create layer.");
217 return false;
218 }
219
220 // Add the layer to the maps.
221 m_umDotMap[szLayerName] = std::vector<std::tuple<double, double, double, double>>();
222 m_umDotLineStyleMap[szLayerName] = std::make_pair(szColorString, bFillMarkerFace);
223 m_umLastDotUpdateTimeMap[szLayerName] = std::chrono::system_clock::now();
224
225 return true;
226 }
227
228
238 bool DeleteLayer(const std::string& szLayerName)
239 {
240 // Check if the layer name exist in the path or dot maps.
241 if (m_umPathMap.find(szLayerName) == m_umPathMap.end() && m_umDotMap.find(szLayerName) == m_umDotMap.end())
242 {
243 // Submit logger message.
244 LOG_WARNING(logging::g_qSharedLogger, "Layer does not exist. Cannot delete layer.");
245 return false;
246 }
247
248 // Remove the appropriate layers from the maps.
249 if (m_umPathMap.find(szLayerName) != m_umPathMap.end())
250 {
251 m_umPathMap.erase(szLayerName);
252 m_umPathLineStyleMap.erase(szLayerName);
253 m_umLastPlotUpdateTimeMap.erase(szLayerName);
254 }
255 if (m_umDotMap.find(szLayerName) != m_umDotMap.end())
256 {
257 m_umDotMap.erase(szLayerName);
258 m_umDotLineStyleMap.erase(szLayerName);
259 m_umLastDotUpdateTimeMap.erase(szLayerName);
260 }
261
262 return true;
263 }
264
265
275 bool ClearLayer(const std::string& szLayerName)
276 {
277 // Check if the layer name exist in the path or dot maps.
278 if (m_umPathMap.find(szLayerName) == m_umPathMap.end() && m_umDotMap.find(szLayerName) == m_umDotMap.end())
279 {
280 // Submit logger message.
281 LOG_WARNING(logging::g_qSharedLogger, "Layer does not exist. Cannot clear layer.");
282 return false;
283 }
284
285 // Clear the appropriate layer.
286 if (m_umPathMap.find(szLayerName) != m_umPathMap.end())
287 {
288 m_umPathMap[szLayerName].clear();
289 }
290 if (m_umDotMap.find(szLayerName) != m_umDotMap.end())
291 {
292 m_umDotMap[szLayerName].clear();
293 }
294
295 return true;
296 }
297
298
310 void AddPathPoint(const geoops::Waypoint& stWaypoint, const std::string& szLayerName, const uint nMaxWaypointsPerSecond = 1)
311 {
312 // Check the update time.
313 if (!this->CheckPathUpdateTime(szLayerName, nMaxWaypointsPerSecond))
314 {
315 // Return if the maximum number of waypoints per second has been exceeded.
316 return;
317 }
318
319 // Check if point is significantly different from the last point to prevent zero-range errors.
320 if (!m_umPathMap[szLayerName].empty())
321 {
322 std::tuple<double, double, double>& stdLastPoint = m_umPathMap[szLayerName].back();
323 double dDistSq = std::pow(std::get<0>(stdLastPoint) - stWaypoint.GetUTMCoordinate().dEasting, 2) +
324 std::pow(std::get<1>(stdLastPoint) - stWaypoint.GetUTMCoordinate().dNorthing, 2);
325 // Ignore updates smaller than 1 cm.
326 if (dDistSq < 0.0001)
327 {
328 return;
329 }
330 }
331
332 // Add the waypoint to the path.
333 m_umPathMap[szLayerName].emplace_back(stWaypoint.GetUTMCoordinate().dEasting,
334 stWaypoint.GetUTMCoordinate().dNorthing,
335 stWaypoint.GetUTMCoordinate().dAltitude);
336
337 // Update the plot.
338 this->UpdatePlot();
339 }
340
341
353 void AddPathPoint(const geoops::UTMCoordinate& stCoordinate, const std::string& szLayerName, const uint nMaxWaypointsPerSecond = 1)
354 {
355 // Check the update time.
356 if (!this->CheckPathUpdateTime(szLayerName, nMaxWaypointsPerSecond))
357 {
358 // Return if the maximum number of waypoints per second has been exceeded.
359 return;
360 }
361
362 // Check if point is significantly different from the last point to prevent zero-range errors.
363 if (!m_umPathMap[szLayerName].empty())
364 {
365 std::tuple<double, double, double>& stdLastPoint = m_umPathMap[szLayerName].back();
366 double dDistSq = std::pow(std::get<0>(stdLastPoint) - stCoordinate.dEasting, 2) + std::pow(std::get<1>(stdLastPoint) - stCoordinate.dNorthing, 2);
367 // Ignore updates smaller than 1 cm.
368 if (dDistSq < 0.0001)
369 {
370 return;
371 }
372 }
373
374 // Add the waypoint to the path.
375 m_umPathMap[szLayerName].emplace_back(stCoordinate.dEasting, stCoordinate.dNorthing, stCoordinate.dAltitude);
376
377 // Update the plot.
378 this->UpdatePlot();
379 }
380
381
393 void AddPathPoint(const geoops::GPSCoordinate& stCoordinate, const std::string& szLayerName, const uint nMaxWaypointsPerSecond = 1)
394 {
395 // Check the update time.
396 if (!this->CheckPathUpdateTime(szLayerName, nMaxWaypointsPerSecond))
397 {
398 // Return if the maximum number of waypoints per second has been exceeded.
399 return;
400 }
401
402 // Create instance variables.
403 geoops::UTMCoordinate stUTMCoordinate = geoops::ConvertGPSToUTM(stCoordinate);
404
405 // Check if point is significantly different from the last point to prevent zero-range errors.
406 if (!m_umPathMap[szLayerName].empty())
407 {
408 std::tuple<double, double, double>& stdLastPoint = m_umPathMap[szLayerName].back();
409 double dDistSq =
410 std::pow(std::get<0>(stdLastPoint) - stUTMCoordinate.dEasting, 2) + std::pow(std::get<1>(stdLastPoint) - stUTMCoordinate.dNorthing, 2);
411 // Ignore updates smaller than 1 cm.
412 if (dDistSq < 0.0001)
413 {
414 return;
415 }
416 }
417
418 // Add the waypoint to the path.
419 m_umPathMap[szLayerName].emplace_back(stUTMCoordinate.dEasting, stUTMCoordinate.dNorthing, stUTMCoordinate.dAltitude);
420
421 // Update the plot.
422 this->UpdatePlot();
423 }
424
425
438 void AddPathPoints(const std::vector<geoops::Waypoint>& stWaypoints, const std::string& szLayerName, const uint unMaxUpdatesPerSecond = 1)
439 {
440 // Check the update time.
441 if (!this->CheckPathUpdateTime(szLayerName, unMaxUpdatesPerSecond))
442 {
443 // Return if the maximum number of waypoints per second has been exceeded.
444 return;
445 }
446
447 // Add the waypoints to the vector or double pairs at the given layer name in the map.
448 for (const geoops::Waypoint& stWaypoint : stWaypoints)
449 {
450 m_umPathMap[szLayerName].emplace_back(stWaypoint.GetUTMCoordinate().dEasting,
451 stWaypoint.GetUTMCoordinate().dNorthing,
452 stWaypoint.GetUTMCoordinate().dAltitude);
453 }
454
455 // Update the plot.
456 this->UpdatePlot();
457 }
458
459
472 void AddPathPoints(const std::vector<geoops::UTMCoordinate>& vCoordinates, const std::string& szLayerName, const uint unMaxUpdatesPerSecond = 1)
473 {
474 // Check the update time.
475 if (!this->CheckPathUpdateTime(szLayerName, unMaxUpdatesPerSecond))
476 {
477 // Return if the maximum number of waypoints per second has been exceeded.
478 return;
479 }
480
481 // Add the waypoints to the vector or double pairs at the given layer name in the map.
482 for (const geoops::UTMCoordinate& stCoordinate : vCoordinates)
483 {
484 m_umPathMap[szLayerName].emplace_back(stCoordinate.dEasting, stCoordinate.dNorthing, stCoordinate.dAltitude);
485 }
486
487 // Update the plot.
488 this->UpdatePlot();
489 }
490
491
504 void AddPathPoints(const std::vector<geoops::GPSCoordinate>& vCoordinates, const std::string& szLayerName, const uint unMaxUpdatesPerSecond = 1)
505 {
506 // Check the update time.
507 if (!this->CheckPathUpdateTime(szLayerName, unMaxUpdatesPerSecond))
508 {
509 // Return if the maximum number of waypoints per second has been exceeded.
510 return;
511 }
512
513 // Add the waypoints to the vector or double pairs at the given layer name in the map.
514 for (const geoops::GPSCoordinate& stCoordinate : vCoordinates)
515 {
516 geoops::UTMCoordinate stUTMCoordinate = geoops::ConvertGPSToUTM(stCoordinate);
517 m_umPathMap[szLayerName].emplace_back(stUTMCoordinate.dEasting, stUTMCoordinate.dNorthing, stUTMCoordinate.dAltitude);
518 }
519
520 // Update the plot.
521 this->UpdatePlot();
522 }
523
524
538 void AddDot(const geoops::Waypoint& stWaypoint, const std::string& szLayerName, const uint nMaxWaypointsPerSecond = 1)
539 {
540 // Check the update time.
541 if (!this->CheckDotUpdateTime(szLayerName, nMaxWaypointsPerSecond))
542 {
543 // Return if the maximum number of waypoints per second has been exceeded.
544 return;
545 }
546
547 // Check the radius of the waypoint. It shouldn't be less than 0.
548 if (stWaypoint.dRadius <= 0)
549 {
550 m_umDotMap[szLayerName].emplace_back(stWaypoint.GetUTMCoordinate().dEasting,
551 stWaypoint.GetUTMCoordinate().dNorthing,
552 stWaypoint.GetUTMCoordinate().dAltitude,
553 5.0);
554 }
555 else
556 {
557 m_umDotMap[szLayerName].emplace_back(stWaypoint.GetUTMCoordinate().dEasting,
558 stWaypoint.GetUTMCoordinate().dNorthing,
559 stWaypoint.GetUTMCoordinate().dAltitude,
560 stWaypoint.dRadius);
561 }
562
563 // Update the plot.
564 this->UpdatePlot();
565 }
566
567
580 void AddDot(const geoops::UTMCoordinate& stCoordinate, const std::string& szLayerName, const uint nMaxWaypointsPerSecond = 1, const double dDotRadius = 5)
581 {
582 // Check the update time.
583 if (!this->CheckDotUpdateTime(szLayerName, nMaxWaypointsPerSecond))
584 {
585 // Return if the maximum number of waypoints per second has been exceeded.
586 return;
587 }
588
589 // Add the waypoint to the path.
590 m_umDotMap[szLayerName].emplace_back(stCoordinate.dEasting, stCoordinate.dNorthing, stCoordinate.dAltitude, dDotRadius);
591
592 // Update the plot.
593 this->UpdatePlot();
594 }
595
596
609 void AddDot(const geoops::GPSCoordinate& stCoordinate, const std::string& szLayerName, const uint nMaxWaypointsPerSecond = 1, const double dDotRadius = 5)
610 {
611 // Check the update time.
612 if (!this->CheckDotUpdateTime(szLayerName, nMaxWaypointsPerSecond))
613 {
614 // Return if the maximum number of waypoints per second has been exceeded.
615 return;
616 }
617
618 // Create instance variables.
619 geoops::UTMCoordinate stUTMCoordinate = geoops::ConvertGPSToUTM(stCoordinate);
620
621 // Add the waypoint to the path.
622 m_umDotMap[szLayerName].emplace_back(stUTMCoordinate.dEasting, stUTMCoordinate.dNorthing, stUTMCoordinate.dAltitude, dDotRadius);
623
624 // Update the plot.
625 this->UpdatePlot();
626 }
627
628
643 void AddDots(const std::vector<geoops::Waypoint>& stWaypoints, const std::string& szLayerName, const uint unMaxUpdatesPerSecond = 1)
644 {
645 // Check the update time.
646 if (!this->CheckDotUpdateTime(szLayerName, unMaxUpdatesPerSecond))
647 {
648 // Return if the maximum number of waypoints per second has been exceeded.
649 return;
650 }
651
652 // Add the waypoints to the vector or double pairs at the given layer name in the map.
653 for (const geoops::Waypoint& stWaypoint : stWaypoints)
654 {
655 // Check the radius of the waypoint. It shouldn't be less than 0.
656 if (stWaypoint.dRadius <= 0)
657 {
658 m_umDotMap[szLayerName].emplace_back(stWaypoint.GetUTMCoordinate().dEasting,
659 stWaypoint.GetUTMCoordinate().dNorthing,
660 stWaypoint.GetUTMCoordinate().dAltitude,
661 5);
662 }
663 else
664 {
665 m_umDotMap[szLayerName].emplace_back(stWaypoint.GetUTMCoordinate().dEasting,
666 stWaypoint.GetUTMCoordinate().dNorthing,
667 stWaypoint.GetUTMCoordinate().dAltitude,
668 stWaypoint.dRadius);
669 }
670 }
671
672 // Update the plot.
673 this->UpdatePlot();
674 }
675
676
690 void AddDots(const std::vector<geoops::UTMCoordinate>& vCoordinates,
691 const std::string& szLayerName,
692 const uint unMaxUpdatesPerSecond = 1,
693 const double dDotRadius = 5)
694 {
695 // Check the update time.
696 if (!this->CheckDotUpdateTime(szLayerName, unMaxUpdatesPerSecond))
697 {
698 // Return if the maximum number of waypoints per second has been exceeded.
699 return;
700 }
701
702 // Add the waypoints to the vector or double pairs at the given layer name in the map.
703 for (const geoops::UTMCoordinate& stCoordinate : vCoordinates)
704 {
705 m_umDotMap[szLayerName].emplace_back(stCoordinate.dEasting, stCoordinate.dNorthing, stCoordinate.dAltitude, dDotRadius);
706 }
707
708 // Update the plot.
709 this->UpdatePlot();
710 }
711
712
726 void AddDots(const std::vector<geoops::GPSCoordinate>& vCoordinates,
727 const std::string& szLayerName,
728 const uint unMaxUpdatesPerSecond = 1,
729 const double dDotRadius = 5)
730 {
731 // Check the update time.
732 if (!this->CheckDotUpdateTime(szLayerName, unMaxUpdatesPerSecond))
733 {
734 // Return if the maximum number of waypoints per second has been exceeded.
735 return;
736 }
737
738 // Add the waypoints to the vector or double pairs at the given layer name in the map.
739 for (const geoops::GPSCoordinate& stCoordinate : vCoordinates)
740 {
741 geoops::UTMCoordinate stUTMCoordinate = geoops::ConvertGPSToUTM(stCoordinate);
742 m_umDotMap[szLayerName].emplace_back(stUTMCoordinate.dEasting, stUTMCoordinate.dNorthing, stUTMCoordinate.dAltitude, dDotRadius);
743 }
744
745 // Update the plot.
746 this->UpdatePlot();
747 }
748
749 private:
750 // Declare private member variables.
751 matplot::figure_handle m_mtRoverPathPlot;
752 matplot::axes_handle m_mtRoverPathAxes;
753 std::unordered_map<std::string, std::string> m_umPathLineStyleMap;
754 std::unordered_map<std::string, std::pair<std::string, bool>> m_umDotLineStyleMap;
755 std::unordered_map<std::string, std::chrono::system_clock::time_point> m_umLastPlotUpdateTimeMap;
756 std::unordered_map<std::string, std::chrono::system_clock::time_point> m_umLastDotUpdateTimeMap;
757 std::unordered_map<std::string, std::vector<std::tuple<double, double, double>>> m_umPathMap;
758 std::unordered_map<std::string, std::vector<std::tuple<double, double, double, double>>> m_umDotMap;
759 std::string m_szPlotTitle;
760 std::string m_szCurrentPlotSavePath;
761 bool m_bEnable3D;
762
763
771 {
772 // Create instance variables.
773 std::vector<std::string> vLayerNames;
774 bool bAnyLayerPlotted = false;
775
776 // Clear the plot.
777 m_mtRoverPathAxes->clear();
778
779 /*
780 PATHS
781 */
782 // Loop through each of the layer name keys in the map.
783 for (const std::pair<const std::string, const std::string>& stdLayer : m_umPathLineStyleMap)
784 {
785 // Check if the vector or coordinates has more than one point.
786 if (m_umPathMap[stdLayer.first].size() > 1)
787 {
788 // Add the layer name to the vector.
789 vLayerNames.push_back(stdLayer.first);
790 bAnyLayerPlotted = true;
791
792 // Check if we are in 3D mode.
793 if (m_bEnable3D)
794 {
795 // Get the x, y, and z coordinates for the layer.
796 std::vector<double> vEasting, vNorthing, vAltitude;
797 for (const std::tuple<double, double, double>& stCoordinate : m_umPathMap[stdLayer.first])
798 {
799 vEasting.push_back(std::get<0>(stCoordinate));
800 vNorthing.push_back(std::get<1>(stCoordinate));
801 vAltitude.push_back(std::get<2>(stCoordinate));
802 }
803
804 // Plot the path in 3D.
805 m_mtRoverPathAxes->plot3(vEasting, vNorthing, vAltitude, std::string_view(stdLayer.second));
806 }
807 else
808 {
809 // Get the x and y coordinates for the layer.
810 std::vector<double> vEasting, vNorthing;
811 for (const std::tuple<double, double, double>& stCoordinate : m_umPathMap[stdLayer.first])
812 {
813 vEasting.push_back(std::get<0>(stCoordinate));
814 vNorthing.push_back(std::get<1>(stCoordinate));
815 }
816
817 // Plot the path.
818 m_mtRoverPathAxes->plot(vEasting, vNorthing, std::string_view(stdLayer.second));
819 }
820
821 // Set the hold to true.
822 m_mtRoverPathAxes->hold(true);
823 }
824 }
825
826 /*
827 DOTS
828 */
829 // Loop through each of the layer name keys in the map.
830 for (const std::pair<const std::string, const std::pair<std::string, bool>>& stdLayer : m_umDotLineStyleMap)
831 {
832 // Check if the vector or coordinates has at least one point.
833 if (m_umDotMap[stdLayer.first].size() > 0)
834 {
835 // Add the layer name to the vector.
836 vLayerNames.push_back(stdLayer.first);
837 bAnyLayerPlotted = true;
838
839 // Check if we are in 3D mode.
840 if (m_bEnable3D)
841 {
842 // Get the x and y coordinates for the layer.
843 std::vector<double> vEasting, vNorthing, vAltitude, vRadius;
844 for (const std::tuple<double, double, double, double>& stCoordinate : m_umDotMap[stdLayer.first])
845 {
846 vEasting.push_back(std::get<0>(stCoordinate));
847 vNorthing.push_back(std::get<1>(stCoordinate));
848 vAltitude.push_back(std::get<2>(stCoordinate));
849 vRadius.push_back(std::get<3>(stCoordinate));
850 }
851
852 // Plot the path.
853 matplot::line_handle mtLineHandle = m_mtRoverPathAxes->scatter3(vEasting, vNorthing, vAltitude, vRadius);
854 // Set the line style and marker face for the layer.
855 mtLineHandle->color(stdLayer.second.first);
856 mtLineHandle->marker_face(stdLayer.second.second);
857 }
858 else
859 {
860 // Get the x and y coordinates for the layer.
861 std::vector<double> vEasting, vNorthing, vRadius;
862 for (const std::tuple<double, double, double, double>& stCoordinate : m_umDotMap[stdLayer.first])
863 {
864 vEasting.push_back(std::get<0>(stCoordinate));
865 vNorthing.push_back(std::get<1>(stCoordinate));
866 vRadius.push_back(std::get<3>(stCoordinate));
867 }
868
869 // Plot the path.
870 matplot::line_handle mtLineHandle = m_mtRoverPathAxes->scatter(vEasting, vNorthing, vRadius);
871 // Set the line style and marker face for the layer.
872 mtLineHandle->color(stdLayer.second.first);
873 mtLineHandle->marker_face(stdLayer.second.second);
874 }
875
876 // Set the hold to true.
877 m_mtRoverPathAxes->hold(true);
878 }
879 }
880
881 // Check if anything was actually plotted.
882 if (!bAnyLayerPlotted)
883 {
884 // If nothing was plotted then don't draw or save.
885 return;
886 }
887
888 // Update legend names.
889 m_mtRoverPathAxes->legend(vLayerNames);
890 matplot::legend_handle mtLegend = m_mtRoverPathAxes->legend();
891 mtLegend->font_size(8);
892 mtLegend->num_columns(2);
893 // Set axis options.
894 m_mtRoverPathAxes->grid(true);
895 m_mtRoverPathAxes->xtickangle(45);
896 m_mtRoverPathAxes->axis(matplot::square);
897 m_mtRoverPathAxes->xtickformat("%.0f"); // No decimal places for x-axis
898 m_mtRoverPathAxes->ytickformat("%.0f"); // No decimal places for y-axis
899 m_mtRoverPathAxes->ztickformat("%.0f"); // No decimal places for z-axis
900 // Set the hold to false.
901 m_mtRoverPathAxes->hold(false);
902 // Plot the path.
903 m_mtRoverPathPlot->draw();
904 m_mtRoverPathPlot->save(m_szCurrentPlotSavePath);
905 }
906
907
923 bool CheckPathUpdateTime(const std::string& szLayerName, const uint unMaxUpdatesPerSecond)
924 {
925 // Check if the layer name exists in the map.
926 if (m_umLastPlotUpdateTimeMap.find(szLayerName) == m_umLastPlotUpdateTimeMap.end())
927 {
928 // Submit logger message.
929 LOG_WARNING(logging::g_qSharedLogger, "Layer does not exist. Cannot add waypoints.");
930 return false;
931 }
932
933 // Check if the maximum number of waypoints per second has been exceeded.
934 if (unMaxUpdatesPerSecond != 0 &&
935 std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now() - m_umLastPlotUpdateTimeMap[szLayerName]).count() <
936 1.0 / unMaxUpdatesPerSecond)
937 {
938 // Return if the maximum number of waypoints per second has been exceeded.
939 return false;
940 }
941
942 // Update the last plot time.
943 m_umLastPlotUpdateTimeMap[szLayerName] = std::chrono::system_clock::now();
944
945 // Return true.
946 return true;
947 }
948
949
965 bool CheckDotUpdateTime(const std::string& szLayerName, const uint unMaxUpdatesPerSecond)
966 {
967 // Check if the layer name exists in the map.
968 if (m_umLastDotUpdateTimeMap.find(szLayerName) == m_umLastDotUpdateTimeMap.end())
969 {
970 // Submit logger message.
971 LOG_WARNING(logging::g_qSharedLogger, "Layer does not exist. Cannot add waypoints.");
972 return false;
973 }
974
975 // Check if the maximum number of waypoints per second has been exceeded.
976 if (unMaxUpdatesPerSecond != 0 &&
977 std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now() - m_umLastDotUpdateTimeMap[szLayerName]).count() <
978 1.0 / unMaxUpdatesPerSecond)
979 {
980 // Return if the maximum number of waypoints per second has been exceeded.
981 return false;
982 }
983
984 // Update the last plot time.
985 m_umLastDotUpdateTimeMap[szLayerName] = std::chrono::system_clock::now();
986
987 // Return true.
988 return true;
989 }
990 };
991 } // namespace graphing
992} // namespace logging
993
994#endif
The PathTracer class is used to trace the path of the rover and plot the path on a graph.
Definition PathTracer.hpp:54
bool CheckPathUpdateTime(const std::string &szLayerName, const uint unMaxUpdatesPerSecond)
Checks the unordered map of last update times for a given layer name and returns true if the time sin...
Definition PathTracer.hpp:923
PathTracer(const std::string &szPlotTitle="Graph", bool bEnable3D=false)
Construct a new Path Tracer object.
Definition PathTracer.hpp:65
~PathTracer()
Destroy the Path Tracer object.
Definition PathTracer.hpp:114
bool CreatePathLayer(const std::string &szLayerName, const std::string &szStyleString="-o")
Add a new draw layer to the plot.
Definition PathTracer.hpp:171
void AddPathPoints(const std::vector< geoops::Waypoint > &stWaypoints, const std::string &szLayerName, const uint unMaxUpdatesPerSecond=1)
Add a waypoint to the path and plot the path. This method has no limit to the number of waypoints tha...
Definition PathTracer.hpp:438
bool ClearLayer(const std::string &szLayerName)
Clear the path or dots of a layer.
Definition PathTracer.hpp:275
void AddDot(const geoops::UTMCoordinate &stCoordinate, const std::string &szLayerName, const uint nMaxWaypointsPerSecond=1, const double dDotRadius=5)
Add a waypoint as a dot to the path. This method has a limit to the number of waypoints that can be a...
Definition PathTracer.hpp:580
void AddDot(const geoops::Waypoint &stWaypoint, const std::string &szLayerName, const uint nMaxWaypointsPerSecond=1)
Add a waypoint as a dot to the path. This method has a limit to the number of waypoints that can be a...
Definition PathTracer.hpp:538
void UpdatePlot()
Update the plot with the new waypoints and redraw the plot.
Definition PathTracer.hpp:770
void AddPathPoint(const geoops::UTMCoordinate &stCoordinate, const std::string &szLayerName, const uint nMaxWaypointsPerSecond=1)
Add a waypoint to the path and plot the path. This method has a limit to the number of waypoints that...
Definition PathTracer.hpp:353
void AddDots(const std::vector< geoops::UTMCoordinate > &vCoordinates, const std::string &szLayerName, const uint unMaxUpdatesPerSecond=1, const double dDotRadius=5)
Add a waypoint as a dot to the path. This method has no limit to the number of waypoints that can be ...
Definition PathTracer.hpp:690
void AddPathPoints(const std::vector< geoops::GPSCoordinate > &vCoordinates, const std::string &szLayerName, const uint unMaxUpdatesPerSecond=1)
Add a waypoint to the path and plot the path. This method has no limit to the number of waypoints tha...
Definition PathTracer.hpp:504
void AddDots(const std::vector< geoops::Waypoint > &stWaypoints, const std::string &szLayerName, const uint unMaxUpdatesPerSecond=1)
Add a waypoint as a dot to the path. This method has no limit to the number of waypoints that can be ...
Definition PathTracer.hpp:643
bool CheckDotUpdateTime(const std::string &szLayerName, const uint unMaxUpdatesPerSecond)
Checks the unordered map of last update times for a given layer name and returns true if the time sin...
Definition PathTracer.hpp:965
void AddPathPoint(const geoops::GPSCoordinate &stCoordinate, const std::string &szLayerName, const uint nMaxWaypointsPerSecond=1)
Add a waypoint to the path and plot the path. This method has a limit to the number of waypoints that...
Definition PathTracer.hpp:393
void AddDot(const geoops::GPSCoordinate &stCoordinate, const std::string &szLayerName, const uint nMaxWaypointsPerSecond=1, const double dDotRadius=5)
Add a waypoint as a dot to the path. This method has a limit to the number of waypoints that can be a...
Definition PathTracer.hpp:609
bool DeleteLayer(const std::string &szLayerName)
Delete a draw layer from the plot.
Definition PathTracer.hpp:238
void AddDots(const std::vector< geoops::GPSCoordinate > &vCoordinates, const std::string &szLayerName, const uint unMaxUpdatesPerSecond=1, const double dDotRadius=5)
Add a waypoint as a dot to the path. This method has no limit to the number of waypoints that can be ...
Definition PathTracer.hpp:726
void AddPathPoints(const std::vector< geoops::UTMCoordinate > &vCoordinates, const std::string &szLayerName, const uint unMaxUpdatesPerSecond=1)
Add a waypoint to the path and plot the path. This method has no limit to the number of waypoints tha...
Definition PathTracer.hpp:472
void AddPathPoint(const geoops::Waypoint &stWaypoint, const std::string &szLayerName, const uint nMaxWaypointsPerSecond=1)
Add a waypoint to the path and plot the path. This method has a limit to the number of waypoints that...
Definition PathTracer.hpp:310
bool CreateDotLayer(const std::string &szLayerName, const std::string &szColorString="blue", const bool bFillMarkerFace=true)
Add a new draw layer to the plot.
Definition PathTracer.hpp:210
uint32_t uint
UTMCoordinate ConvertGPSToUTM(const GPSCoordinate &stGPSCoord)
Given a GPS coordinate, convert to UTM and create a new UTMCoordinate object.
Definition GeospatialOperations.hpp:333
Namespace containing all global type/structs that will be used project wide for logging.
Definition AutonomyLogging.cpp:33
This struct stores/contains information about a GPS data.
Definition GeospatialOperations.hpp:100
This struct stores/contains information about a UTM coordinate.
Definition GeospatialOperations.hpp:211
This struct is used by the WaypointHandler class to store location, size, and type information about ...
Definition GeospatialOperations.hpp:423
const geoops::UTMCoordinate & GetUTMCoordinate() const
Accessor for the geoops::UTMCoordinate member variable.
Definition GeospatialOperations.hpp:508