12#ifndef PATH_TRACER_HPP
13#define PATH_TRACER_HPP
15#include "../GeospatialOperations.hpp"
19#include <matplot/matplot.h>
64 PathTracer(
const std::string& szPlotTitle =
"Graph",
bool bEnable3D =
false)
67 m_mtRoverPathPlot = matplot::figure(
true);
68 m_mtRoverPathAxes = m_mtRoverPathPlot->current_axes();
69 m_szPlotTitle = szPlotTitle;
70 m_bEnable3D = bEnable3D;
73 std::string szPlotSavePath = logging::g_szLoggingOutputPath +
"/path_plots/" + m_szPlotTitle;
74 m_szCurrentPlotSavePath = logging::g_szLoggingOutputPath +
"/path_plots/CurrentPlot.png";
76 while (std::filesystem::exists(szPlotSavePath + std::to_string(nFileNum) +
".png"))
81 szPlotSavePath = szPlotSavePath + std::to_string(nFileNum);
83 if (!std::filesystem::exists(logging::g_szLoggingOutputPath +
"/path_plots"))
85 std::filesystem::create_directory(logging::g_szLoggingOutputPath +
"/path_plots");
89 m_mtRoverPathPlot->backend()->output(szPlotSavePath +
".png");
92 if (m_szPlotTitle.empty())
95 LOG_WARNING(logging::g_qSharedLogger,
"Plot title is empty. Setting title to default.");
96 m_szPlotTitle =
"RoverPath";
100 m_mtRoverPathPlot->title(m_szPlotTitle);
101 m_mtRoverPathAxes->xlabel(
"Easting");
102 m_mtRoverPathAxes->ylabel(
"Northing");
103 m_mtRoverPathAxes->zlabel(
"Altitude");
170 bool CreatePathLayer(
const std::string& szLayerName,
const std::string& szStyleString =
"-o")
173 if (m_umPathMap.find(szLayerName) != m_umPathMap.end())
176 LOG_WARNING(logging::g_qSharedLogger,
"Layer already exists. Cannot create layer.");
181 m_umPathMap[szLayerName] = std::vector<std::tuple<double, double, double>>();
182 m_umPathLineStyleMap[szLayerName] = szStyleString;
183 m_umLastPlotUpdateTimeMap[szLayerName] = std::chrono::system_clock::now();
209 bool CreateDotLayer(
const std::string& szLayerName,
const std::string& szColorString =
"blue",
const bool bFillMarkerFace =
true)
212 if (m_umDotMap.find(szLayerName) != m_umDotMap.end())
215 LOG_WARNING(logging::g_qSharedLogger,
"Layer already exists. Cannot create layer.");
220 m_umDotMap[szLayerName] = std::vector<std::tuple<double, double, double, double>>();
221 m_umDotLineStyleMap[szLayerName] = std::make_pair(szColorString, bFillMarkerFace);
222 m_umLastDotUpdateTimeMap[szLayerName] = std::chrono::system_clock::now();
240 if (m_umPathMap.find(szLayerName) == m_umPathMap.end() && m_umDotMap.find(szLayerName) == m_umDotMap.end())
243 LOG_WARNING(logging::g_qSharedLogger,
"Layer does not exist. Cannot delete layer.");
248 if (m_umPathMap.find(szLayerName) != m_umPathMap.end())
250 m_umPathMap.erase(szLayerName);
251 m_umPathLineStyleMap.erase(szLayerName);
252 m_umLastPlotUpdateTimeMap.erase(szLayerName);
254 if (m_umDotMap.find(szLayerName) != m_umDotMap.end())
256 m_umDotMap.erase(szLayerName);
257 m_umDotLineStyleMap.erase(szLayerName);
258 m_umLastDotUpdateTimeMap.erase(szLayerName);
277 if (m_umPathMap.find(szLayerName) == m_umPathMap.end() && m_umDotMap.find(szLayerName) == m_umDotMap.end())
280 LOG_WARNING(logging::g_qSharedLogger,
"Layer does not exist. Cannot clear layer.");
285 if (m_umPathMap.find(szLayerName) != m_umPathMap.end())
287 m_umPathMap[szLayerName].clear();
289 if (m_umDotMap.find(szLayerName) != m_umDotMap.end())
291 m_umDotMap[szLayerName].clear();
319 m_umPathMap[szLayerName].emplace_back(stWaypoint.
GetUTMCoordinate().dEasting,
349 m_umPathMap[szLayerName].emplace_back(stCoordinate.dEasting, stCoordinate.dNorthing, stCoordinate.dAltitude);
380 m_umPathMap[szLayerName].emplace_back(stUTMCoordinate.dEasting, stUTMCoordinate.dNorthing, stUTMCoordinate.dAltitude);
399 void AddPathPoints(
const std::vector<geoops::Waypoint>& stWaypoints,
const std::string& szLayerName,
const uint unMaxUpdatesPerSecond = 1)
411 m_umPathMap[szLayerName].emplace_back(stWaypoint.GetUTMCoordinate().dEasting,
412 stWaypoint.GetUTMCoordinate().dNorthing,
413 stWaypoint.GetUTMCoordinate().dAltitude);
433 void AddPathPoints(
const std::vector<geoops::UTMCoordinate>& vCoordinates,
const std::string& szLayerName,
const uint unMaxUpdatesPerSecond = 1)
445 m_umPathMap[szLayerName].emplace_back(stCoordinate.dEasting, stCoordinate.dNorthing, stCoordinate.dAltitude);
465 void AddPathPoints(
const std::vector<geoops::GPSCoordinate>& vCoordinates,
const std::string& szLayerName,
const uint unMaxUpdatesPerSecond = 1)
478 m_umPathMap[szLayerName].emplace_back(stUTMCoordinate.dEasting, stUTMCoordinate.dNorthing, stUTMCoordinate.dAltitude);
509 if (stWaypoint.dRadius <= 0)
511 m_umDotMap[szLayerName].emplace_back(stWaypoint.
GetUTMCoordinate().dEasting,
518 m_umDotMap[szLayerName].emplace_back(stWaypoint.
GetUTMCoordinate().dEasting,
551 m_umDotMap[szLayerName].emplace_back(stCoordinate.dEasting, stCoordinate.dNorthing, stCoordinate.dAltitude, dDotRadius);
583 m_umDotMap[szLayerName].emplace_back(stUTMCoordinate.dEasting, stUTMCoordinate.dNorthing, stUTMCoordinate.dAltitude, dDotRadius);
604 void AddDots(
const std::vector<geoops::Waypoint>& stWaypoints,
const std::string& szLayerName,
const uint unMaxUpdatesPerSecond = 1)
617 if (stWaypoint.dRadius <= 0)
619 m_umDotMap[szLayerName].emplace_back(stWaypoint.GetUTMCoordinate().dEasting,
620 stWaypoint.GetUTMCoordinate().dNorthing,
621 stWaypoint.GetUTMCoordinate().dAltitude,
626 m_umDotMap[szLayerName].emplace_back(stWaypoint.GetUTMCoordinate().dEasting,
627 stWaypoint.GetUTMCoordinate().dNorthing,
628 stWaypoint.GetUTMCoordinate().dAltitude,
651 void AddDots(
const std::vector<geoops::UTMCoordinate>& vCoordinates,
652 const std::string& szLayerName,
653 const uint unMaxUpdatesPerSecond = 1,
654 const double dDotRadius = 5)
666 m_umDotMap[szLayerName].emplace_back(stCoordinate.dEasting, stCoordinate.dNorthing, stCoordinate.dAltitude, dDotRadius);
687 void AddDots(
const std::vector<geoops::GPSCoordinate>& vCoordinates,
688 const std::string& szLayerName,
689 const uint unMaxUpdatesPerSecond = 1,
690 const double dDotRadius = 5)
703 m_umDotMap[szLayerName].emplace_back(stUTMCoordinate.dEasting, stUTMCoordinate.dNorthing, stUTMCoordinate.dAltitude, dDotRadius);
712 matplot::figure_handle m_mtRoverPathPlot;
713 matplot::axes_handle m_mtRoverPathAxes;
714 std::unordered_map<std::string, std::string> m_umPathLineStyleMap;
715 std::unordered_map<std::string, std::pair<std::string, bool>> m_umDotLineStyleMap;
716 std::unordered_map<std::string, std::chrono::system_clock::time_point> m_umLastPlotUpdateTimeMap;
717 std::unordered_map<std::string, std::chrono::system_clock::time_point> m_umLastDotUpdateTimeMap;
718 std::unordered_map<std::string, std::vector<std::tuple<double, double, double>>> m_umPathMap;
719 std::unordered_map<std::string, std::vector<std::tuple<double, double, double, double>>> m_umDotMap;
720 std::string m_szPlotTitle;
721 std::string m_szCurrentPlotSavePath;
734 std::vector<std::string> vLayerNames;
737 m_mtRoverPathAxes->clear();
743 for (
const std::pair<const std::string, const std::string>& stdLayer : m_umPathLineStyleMap)
746 if (m_umPathMap[stdLayer.first].size() > 1)
749 vLayerNames.push_back(stdLayer.first);
755 std::vector<double> vEasting, vNorthing, vAltitude;
756 for (
const std::tuple<double, double, double>& stCoordinate : m_umPathMap[stdLayer.first])
758 vEasting.push_back(std::get<0>(stCoordinate));
759 vNorthing.push_back(std::get<1>(stCoordinate));
760 vAltitude.push_back(std::get<2>(stCoordinate));
764 m_mtRoverPathAxes->plot3(vEasting, vNorthing, vAltitude, std::string_view(stdLayer.second));
769 std::vector<double> vEasting, vNorthing;
770 for (
const std::tuple<double, double, double>& stCoordinate : m_umPathMap[stdLayer.first])
772 vEasting.push_back(std::get<0>(stCoordinate));
773 vNorthing.push_back(std::get<1>(stCoordinate));
777 m_mtRoverPathAxes->plot(vEasting, vNorthing, std::string_view(stdLayer.second));
781 m_mtRoverPathAxes->hold(
true);
789 for (
const std::pair<
const std::string,
const std::pair<std::string, bool>>& stdLayer : m_umDotLineStyleMap)
792 if (m_umDotMap[stdLayer.first].size() > 0)
795 vLayerNames.push_back(stdLayer.first);
801 std::vector<double> vEasting, vNorthing, vAltitude, vRadius;
802 for (
const std::tuple<double, double, double, double>& stCoordinate : m_umDotMap[stdLayer.first])
804 vEasting.push_back(std::get<0>(stCoordinate));
805 vNorthing.push_back(std::get<1>(stCoordinate));
806 vAltitude.push_back(std::get<2>(stCoordinate));
807 vRadius.push_back(std::get<3>(stCoordinate));
811 matplot::line_handle mtLineHandle = m_mtRoverPathAxes->scatter3(vEasting, vNorthing, vAltitude, vRadius);
813 mtLineHandle->color(stdLayer.second.first);
814 mtLineHandle->marker_face(stdLayer.second.second);
819 std::vector<double> vEasting, vNorthing, vRadius;
820 for (
const std::tuple<double, double, double, double>& stCoordinate : m_umDotMap[stdLayer.first])
822 vEasting.push_back(std::get<0>(stCoordinate));
823 vNorthing.push_back(std::get<1>(stCoordinate));
824 vRadius.push_back(std::get<3>(stCoordinate));
828 matplot::line_handle mtLineHandle = m_mtRoverPathAxes->scatter(vEasting, vNorthing, vRadius);
830 mtLineHandle->color(stdLayer.second.first);
831 mtLineHandle->marker_face(stdLayer.second.second);
835 m_mtRoverPathAxes->hold(
true);
840 m_mtRoverPathAxes->legend(vLayerNames);
841 matplot::legend_handle mtLegend = m_mtRoverPathAxes->legend();
842 mtLegend->font_size(8);
843 mtLegend->num_columns(2);
845 m_mtRoverPathAxes->grid(
true);
846 m_mtRoverPathAxes->xtickangle(45);
847 m_mtRoverPathAxes->axis(matplot::square);
848 m_mtRoverPathAxes->xtickformat(
"%.0f");
849 m_mtRoverPathAxes->ytickformat(
"%.0f");
850 m_mtRoverPathAxes->ztickformat(
"%.0f");
852 m_mtRoverPathAxes->hold(
false);
854 m_mtRoverPathPlot->draw();
855 m_mtRoverPathPlot->save(m_szCurrentPlotSavePath);
877 if (m_umLastPlotUpdateTimeMap.find(szLayerName) == m_umLastPlotUpdateTimeMap.end())
880 LOG_WARNING(logging::g_qSharedLogger,
"Layer does not exist. Cannot add waypoints.");
885 if (unMaxUpdatesPerSecond != 0 &&
886 std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now() - m_umLastPlotUpdateTimeMap[szLayerName]).count() <
887 1.0 / unMaxUpdatesPerSecond)
894 m_umLastPlotUpdateTimeMap[szLayerName] = std::chrono::system_clock::now();
919 if (m_umLastDotUpdateTimeMap.find(szLayerName) == m_umLastDotUpdateTimeMap.end())
922 LOG_WARNING(logging::g_qSharedLogger,
"Layer does not exist. Cannot add waypoints.");
927 if (unMaxUpdatesPerSecond != 0 &&
928 std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now() - m_umLastDotUpdateTimeMap[szLayerName]).count() <
929 1.0 / unMaxUpdatesPerSecond)
936 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:53
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:874
PathTracer(const std::string &szPlotTitle="Graph", bool bEnable3D=false)
Construct a new Path Tracer object.
Definition PathTracer.hpp:64
~PathTracer()
Destroy the Path Tracer object.
Definition PathTracer.hpp:113
bool CreatePathLayer(const std::string &szLayerName, const std::string &szStyleString="-o")
Add a new draw layer to the plot.
Definition PathTracer.hpp:170
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:399
bool ClearLayer(const std::string &szLayerName)
Clear the path or dots of a layer.
Definition PathTracer.hpp:274
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:541
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:499
void UpdatePlot()
Update the plot with the new waypoints and redraw the plot.
Definition PathTracer.hpp:731
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:339
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:651
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:465
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:604
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:916
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:367
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:570
bool DeleteLayer(const std::string &szLayerName)
Delete a draw layer from the plot.
Definition PathTracer.hpp:237
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:687
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:433
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:309
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:209
UTMCoordinate ConvertGPSToUTM(const GPSCoordinate &stGPSCoord)
Given a GPS coordinate, convert to UTM and create a new UTMCoordinate object.
Definition GeospatialOperations.hpp:332
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:99
This struct stores/contains information about a UTM coordinate.
Definition GeospatialOperations.hpp:210
This struct is used by the WaypointHandler class to store location, size, and type information about ...
Definition GeospatialOperations.hpp:422
const geoops::UTMCoordinate & GetUTMCoordinate() const
Accessor for the geoops::UTMCoordinate member variable.
Definition GeospatialOperations.hpp:507