Incremental Merge of A* Testing Branch | New Graphing Utility!

[ClayJay3]

Description

This PR is an incremental merge of the feature/astar-testing branch. The branch was originally created to verify the functionality of the A* and Stanley Controller algorithms, but I quickly realized that we desperately needed a good way to visualize what the algorithms and rover were actually doing. I decided to do something similar to what the python codebase had, but it's much more modular and easy to use. A new library has been added: matplotplusplus. This library provides a similar interface to python's matplotlib, but is actually completely different and uses the gnuplot-nox package in the background the generate the graphs. It is much faster and works almost identically.

Changes Made

• Added new matplotplusplus library.
• Added new packages to the Dockerfiles (docker images need rebuilt)
• Wrote a new PredictiveStanleyController based off this whitepaper
• Wrote PathTracer2D.hpp and PlotsAndGraphs.hpp util classes
• Add CoPilot extension to the devcontainer.json

How It Works

1. Create the plot object and give it a title:
   m_pRoverPathPlot = std::make_unique<logging::graphing::PathTracer>("NavigatingRoverPath");
2. Add graphing layers for paths and scatters:

       // Add the search and rover path layers to the plot.
       m_pRoverPathPlot->CreatePathLayer("SpiralSearchPattern", "-o");
       m_pRoverPathPlot->CreateDotLayer("VerticalZigZagSearchPattern", "yellow");
       m_pRoverPathPlot->CreateDotLayer("HorizontalZigZagSearchPattern", "green");
       m_pRoverPathPlot->CreatePathLayer("RoverPath", "-.r*");
   

3. Add multiple points at once:
   m_pRoverPathPlot->AddPathPoints(m_vSearchPath, "SpiralSearchPattern"); // Line
   m_pRoverPathPlot->AddDots(m_vSearchPath, "VerticalZigZagSearchPattern"); // Scatter
4. Add one point at a time:
   m_pRoverPathPlot->AddPathPoint(stCurrentRoverPose.GetUTMCoordinate(), "RoverPath"); // Line
   m_pRoverPathPlot->AddDot(stCurrentRoverPose.GetUTMCoordinate(), "VerticalZigZagSearchPattern"); // Scatter
5. Graphs are automatically named according to their title and placed in the appropriate log folder, so no need to save or manually destroy the plot object.

Adding points are automatically rate limited to avoid updating the graph too many times per second. If you want to add a list of points in bulk to the graph, you can use the AddPathPoints(). If you have a line path that you want to update periodically, say a rover path, then you can just call AddPathPoint(). The user doesn't need to add any timing code, they can call AddPathPoints() or AddPathPoint() every iteration, and it will rate-limit the graph for them. If the user want to forgo the rate limit or change it, then they can pass the desired update interval limit as a 3rd argument to the function, see example below:

// Set interval limit to 0, so we can add as many points as we want with no rate limit.
m_pRoverPathPlot->AddPathPoint(stCurrentRoverPose.GetUTMCoordinate(), "RoverPath", 0);

Testing

Leads should determine if the graph util file needs unit tests or something,
I have yet to write unit tests for the PredictiveStanleyController class. Honestly, both version of stanley don't work very well at the moment, so I'm not sure if we're going to keep stanley. I'm going to hold off on the unit tests until I know they're worth writing.

Screenshots/Video (if applicable)

The generated graphs are written to the current log folder in a new path_plots directory:

Each time a new Navigating or SearchPattern state is made new graphs are generated and old ones from past states remain saved in the logs directory. You can also abort/save a state and then resume it and it will continue to update the same graph from before. Graphs are only close/opened when a state is destroyed/created respectively.

Additional Context

I think this will be very nice during testing and even during competition. Since we'll have a good way to sort of look into what autonomy is computing and what it's doing. The graphs can opened normally in VSCode (also works through a remote VSCode connection via SSH) and viewed live while the rover is driving.

I also measured it, the bandwidth usage of viewing the graph through VSCode is very low, it sends less than 100kb/s, so it shouldn't really affect us at competition unless are signals are dog slow.

[deepsource-io]

Here's the code health analysis summary for commits c56f502..97c3365. View details on DeepSource ↗.

Analysis Summary

Analyzer	Status	Summary	Link
Test coverage	✅ Success		View Check ↗
Shell	✅ Success		View Check ↗
Secrets	✅ Success		View Check ↗
Python	✅ Success		View Check ↗
Docker	✅ Success		View Check ↗
C & C++	✅ Success		View Check ↗

Code Coverage Report

Metric	Aggregate	C & C++
Branch Coverage	13.2% (up 2.6% from development)	13.2% (up 2.6% from development)
Condition Coverage	13.2% (up 2.6% from development)	13.2% (up 2.6% from development)
Composite Coverage	19% (up 3.3% from development)	19% (up 3.3% from development)
Line Coverage	27.5% (up 4.3% from development)	27.5% (up 4.3% from development)
New Branch Coverage	36.7%	36.7%
New Condition Coverage	36.7%	36.7%
New Composite Coverage	49.6%	49.6%
New Line Coverage	69.5%	69.5%

---

💡 If you’re a repository administrator, you can configure the quality gates from the settings.