12#ifndef PATH_TRACER_HPP
13#define PATH_TRACER_HPP
15#include "../GeospatialOperations.hpp"
20#include <matplot/matplot.h>
65 PathTracer(
const std::string& szPlotTitle =
"Graph",
bool bEnable3D =
false)
68 m_mtRoverPathPlot = matplot::figure(
true);
69 m_mtRoverPathAxes = m_mtRoverPathPlot->current_axes();
70 m_szPlotTitle = szPlotTitle;
71 m_bEnable3D = bEnable3D;
74 std::string szPlotSavePath = logging::g_szLoggingOutputPath +
"/path_plots/" + m_szPlotTitle;
75 m_szCurrentPlotSavePath = logging::g_szLoggingOutputPath +
"/path_plots/CurrentPlot.png";
77 while (std::filesystem::exists(szPlotSavePath + std::to_string(nFileNum) +
".png"))
82 szPlotSavePath = szPlotSavePath + std::to_string(nFileNum);
84 if (!std::filesystem::exists(logging::g_szLoggingOutputPath +
"/path_plots"))
86 std::filesystem::create_directory(logging::g_szLoggingOutputPath +
"/path_plots");
90 m_mtRoverPathPlot->backend()->output(szPlotSavePath +
".png");
93 if (m_szPlotTitle.empty())
96 LOG_WARNING(logging::g_qSharedLogger,
"Plot title is empty. Setting title to default.");
97 m_szPlotTitle =
"RoverPath";
101 m_mtRoverPathPlot->title(m_szPlotTitle);
102 m_mtRoverPathAxes->xlabel(
"Easting");
103 m_mtRoverPathAxes->ylabel(
"Northing");
104 m_mtRoverPathAxes->zlabel(
"Altitude");
171 bool CreatePathLayer(
const std::string& szLayerName,
const std::string& szStyleString =
"-o")
174 if (m_umPathMap.find(szLayerName) != m_umPathMap.end())
177 LOG_WARNING(logging::g_qSharedLogger,
"Layer already exists. Cannot create layer.");
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();
210 bool CreateDotLayer(
const std::string& szLayerName,
const std::string& szColorString =
"blue",
const bool bFillMarkerFace =
true)
213 if (m_umDotMap.find(szLayerName) != m_umDotMap.end())
216 LOG_WARNING(logging::g_qSharedLogger,
"Layer already exists. Cannot create layer.");
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();
241 if (m_umPathMap.find(szLayerName) == m_umPathMap.end() && m_umDotMap.find(szLayerName) == m_umDotMap.end())
244 LOG_WARNING(logging::g_qSharedLogger,
"Layer does not exist. Cannot delete layer.");
249 if (m_umPathMap.find(szLayerName) != m_umPathMap.end())
251 m_umPathMap.erase(szLayerName);
252 m_umPathLineStyleMap.erase(szLayerName);
253 m_umLastPlotUpdateTimeMap.erase(szLayerName);
255 if (m_umDotMap.find(szLayerName) != m_umDotMap.end())
257 m_umDotMap.erase(szLayerName);
258 m_umDotLineStyleMap.erase(szLayerName);
259 m_umLastDotUpdateTimeMap.erase(szLayerName);
278 if (m_umPathMap.find(szLayerName) == m_umPathMap.end() && m_umDotMap.find(szLayerName) == m_umDotMap.end())
281 LOG_WARNING(logging::g_qSharedLogger,
"Layer does not exist. Cannot clear layer.");
286 if (m_umPathMap.find(szLayerName) != m_umPathMap.end())
288 m_umPathMap[szLayerName].clear();
290 if (m_umDotMap.find(szLayerName) != m_umDotMap.end())
292 m_umDotMap[szLayerName].clear();
320 if (!m_umPathMap[szLayerName].empty())
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);
326 if (dDistSq < 0.0001)
333 m_umPathMap[szLayerName].emplace_back(stWaypoint.
GetUTMCoordinate().dEasting,
363 if (!m_umPathMap[szLayerName].empty())
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);
368 if (dDistSq < 0.0001)
375 m_umPathMap[szLayerName].emplace_back(stCoordinate.dEasting, stCoordinate.dNorthing, stCoordinate.dAltitude);
406 if (!m_umPathMap[szLayerName].empty())
408 std::tuple<double, double, double>& stdLastPoint = m_umPathMap[szLayerName].back();
410 std::pow(std::get<0>(stdLastPoint) - stUTMCoordinate.dEasting, 2) + std::pow(std::get<1>(stdLastPoint) - stUTMCoordinate.dNorthing, 2);
412 if (dDistSq < 0.0001)
419 m_umPathMap[szLayerName].emplace_back(stUTMCoordinate.dEasting, stUTMCoordinate.dNorthing, stUTMCoordinate.dAltitude);
438 void AddPathPoints(
const std::vector<geoops::Waypoint>& stWaypoints,
const std::string& szLayerName,
const uint unMaxUpdatesPerSecond = 1)
450 m_umPathMap[szLayerName].emplace_back(stWaypoint.GetUTMCoordinate().dEasting,
451 stWaypoint.GetUTMCoordinate().dNorthing,
452 stWaypoint.GetUTMCoordinate().dAltitude);
472 void AddPathPoints(
const std::vector<geoops::UTMCoordinate>& vCoordinates,
const std::string& szLayerName,
const uint unMaxUpdatesPerSecond = 1)
484 m_umPathMap[szLayerName].emplace_back(stCoordinate.dEasting, stCoordinate.dNorthing, stCoordinate.dAltitude);
504 void AddPathPoints(
const std::vector<geoops::GPSCoordinate>& vCoordinates,
const std::string& szLayerName,
const uint unMaxUpdatesPerSecond = 1)
517 m_umPathMap[szLayerName].emplace_back(stUTMCoordinate.dEasting, stUTMCoordinate.dNorthing, stUTMCoordinate.dAltitude);
548 if (stWaypoint.dRadius <= 0)
550 m_umDotMap[szLayerName].emplace_back(stWaypoint.
GetUTMCoordinate().dEasting,
557 m_umDotMap[szLayerName].emplace_back(stWaypoint.
GetUTMCoordinate().dEasting,
590 m_umDotMap[szLayerName].emplace_back(stCoordinate.dEasting, stCoordinate.dNorthing, stCoordinate.dAltitude, dDotRadius);
622 m_umDotMap[szLayerName].emplace_back(stUTMCoordinate.dEasting, stUTMCoordinate.dNorthing, stUTMCoordinate.dAltitude, dDotRadius);
643 void AddDots(
const std::vector<geoops::Waypoint>& stWaypoints,
const std::string& szLayerName,
const uint unMaxUpdatesPerSecond = 1)
656 if (stWaypoint.dRadius <= 0)
658 m_umDotMap[szLayerName].emplace_back(stWaypoint.GetUTMCoordinate().dEasting,
659 stWaypoint.GetUTMCoordinate().dNorthing,
660 stWaypoint.GetUTMCoordinate().dAltitude,
665 m_umDotMap[szLayerName].emplace_back(stWaypoint.GetUTMCoordinate().dEasting,
666 stWaypoint.GetUTMCoordinate().dNorthing,
667 stWaypoint.GetUTMCoordinate().dAltitude,
690 void AddDots(
const std::vector<geoops::UTMCoordinate>& vCoordinates,
691 const std::string& szLayerName,
692 const uint unMaxUpdatesPerSecond = 1,
693 const double dDotRadius = 5)
705 m_umDotMap[szLayerName].emplace_back(stCoordinate.dEasting, stCoordinate.dNorthing, stCoordinate.dAltitude, dDotRadius);
726 void AddDots(
const std::vector<geoops::GPSCoordinate>& vCoordinates,
727 const std::string& szLayerName,
728 const uint unMaxUpdatesPerSecond = 1,
729 const double dDotRadius = 5)
742 m_umDotMap[szLayerName].emplace_back(stUTMCoordinate.dEasting, stUTMCoordinate.dNorthing, stUTMCoordinate.dAltitude, dDotRadius);
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;
773 std::vector<std::string> vLayerNames;
774 bool bAnyLayerPlotted =
false;
777 m_mtRoverPathAxes->clear();
783 for (
const std::pair<const std::string, const std::string>& stdLayer : m_umPathLineStyleMap)
786 if (m_umPathMap[stdLayer.first].size() > 1)
789 vLayerNames.push_back(stdLayer.first);
790 bAnyLayerPlotted =
true;
796 std::vector<double> vEasting, vNorthing, vAltitude;
797 for (
const std::tuple<double, double, double>& stCoordinate : m_umPathMap[stdLayer.first])
799 vEasting.push_back(std::get<0>(stCoordinate));
800 vNorthing.push_back(std::get<1>(stCoordinate));
801 vAltitude.push_back(std::get<2>(stCoordinate));
805 m_mtRoverPathAxes->plot3(vEasting, vNorthing, vAltitude, std::string_view(stdLayer.second));
810 std::vector<double> vEasting, vNorthing;
811 for (
const std::tuple<double, double, double>& stCoordinate : m_umPathMap[stdLayer.first])
813 vEasting.push_back(std::get<0>(stCoordinate));
814 vNorthing.push_back(std::get<1>(stCoordinate));
818 m_mtRoverPathAxes->plot(vEasting, vNorthing, std::string_view(stdLayer.second));
822 m_mtRoverPathAxes->hold(
true);
830 for (
const std::pair<
const std::string,
const std::pair<std::string, bool>>& stdLayer : m_umDotLineStyleMap)
833 if (m_umDotMap[stdLayer.first].size() > 0)
836 vLayerNames.push_back(stdLayer.first);
837 bAnyLayerPlotted =
true;
843 std::vector<double> vEasting, vNorthing, vAltitude, vRadius;
844 for (
const std::tuple<double, double, double, double>& stCoordinate : m_umDotMap[stdLayer.first])
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));
853 matplot::line_handle mtLineHandle = m_mtRoverPathAxes->scatter3(vEasting, vNorthing, vAltitude, vRadius);
855 mtLineHandle->color(stdLayer.second.first);
856 mtLineHandle->marker_face(stdLayer.second.second);
861 std::vector<double> vEasting, vNorthing, vRadius;
862 for (
const std::tuple<double, double, double, double>& stCoordinate : m_umDotMap[stdLayer.first])
864 vEasting.push_back(std::get<0>(stCoordinate));
865 vNorthing.push_back(std::get<1>(stCoordinate));
866 vRadius.push_back(std::get<3>(stCoordinate));
870 matplot::line_handle mtLineHandle = m_mtRoverPathAxes->scatter(vEasting, vNorthing, vRadius);
872 mtLineHandle->color(stdLayer.second.first);
873 mtLineHandle->marker_face(stdLayer.second.second);
877 m_mtRoverPathAxes->hold(
true);
882 if (!bAnyLayerPlotted)
889 m_mtRoverPathAxes->legend(vLayerNames);
890 matplot::legend_handle mtLegend = m_mtRoverPathAxes->legend();
891 mtLegend->font_size(8);
892 mtLegend->num_columns(2);
894 m_mtRoverPathAxes->grid(
true);
895 m_mtRoverPathAxes->xtickangle(45);
896 m_mtRoverPathAxes->axis(matplot::square);
897 m_mtRoverPathAxes->xtickformat(
"%.0f");
898 m_mtRoverPathAxes->ytickformat(
"%.0f");
899 m_mtRoverPathAxes->ztickformat(
"%.0f");
901 m_mtRoverPathAxes->hold(
false);
903 m_mtRoverPathPlot->draw();
904 m_mtRoverPathPlot->save(m_szCurrentPlotSavePath);
926 if (m_umLastPlotUpdateTimeMap.find(szLayerName) == m_umLastPlotUpdateTimeMap.end())
929 LOG_WARNING(logging::g_qSharedLogger,
"Layer does not exist. Cannot add waypoints.");
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)
943 m_umLastPlotUpdateTimeMap[szLayerName] = std::chrono::system_clock::now();
968 if (m_umLastDotUpdateTimeMap.find(szLayerName) == m_umLastDotUpdateTimeMap.end())
971 LOG_WARNING(logging::g_qSharedLogger,
"Layer does not exist. Cannot add waypoints.");
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)
985 m_umLastDotUpdateTimeMap[szLayerName] = std::chrono::system_clock::now();
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
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