Merge pull request #365 from MissouriMRDT/topic/astar-testing

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

.devcontainer/Jammy.dockerfile

@@ -46,7 +46,7 @@ RUN apt-get update && apt-get install --no-install-recommends -y iputils-ping \
     yasm libatlas-base-dev gfortran libpq-dev libpostproc-dev \
     libxine2-dev libglew-dev libtiff5-dev zlib1g-dev cowsay lolcat locales usbutils \
     libeigen3-dev python3-dev python3-pip python3-numpy libx11-dev xauth libssl-dev \
-    valgrind doxygen graphviz htop nano fortune fortunes \
+    valgrind doxygen graphviz htop nano fortune fortunes gnuplot-nox \
     vim-common gasket-dkms nlohmann-json3-dev gcovr lcov curl \
     libaom-dev libass-dev libfdk-aac-dev libdav1d-dev libmp3lame-dev \
     libopus-dev libvorbis-dev libvpx-dev libx264-dev libx265-dev
@@ -105,6 +105,18 @@ RUN wget -q https://github.com/MissouriMRDT/Autonomy_Packages/raw/main/pytorch/a
     dpkg -i pytorch_${TORCH_VERSION}_amd64.deb && \
     rm pytorch_${TORCH_VERSION}_amd64.deb
 
+# Install Tensorflow.
+ARG TENSORFLOW_VERSION="2.15.0"
+RUN wget -q https://github.com/MissouriMRDT/Autonomy_Packages/raw/main/tensorflow/amd64/tensorflow_${TENSORFLOW_VERSION}_amd64.deb && \
+    dpkg -i tensorflow_${TENSORFLOW_VERSION}_amd64.deb && \
+    rm tensorflow_${TENSORFLOW_VERSION}_amd64.deb
+
+# Install FFMPEG
+ARG FFMPEG_VERSION="7.1"
+RUN wget -q https://github.com/MissouriMRDT/Autonomy_Packages/raw/main/ffmpeg/amd64/ffmpeg_${FFMPEG_VERSION}_amd64.deb && \
+    dpkg -i ffmpeg_${FFMPEG_VERSION}_amd64.deb && \
+    rm ffmpeg_${FFMPEG_VERSION}_amd64.deb
+
 # Install Abseil.
 ARG ABSEIL_VERSION="20230802.1"
 RUN wget -q https://github.com/MissouriMRDT/Autonomy_Packages/raw/main/abseil/amd64/abseil_${ABSEIL_VERSION}_amd64.deb && \
@@ -117,23 +129,17 @@ RUN wget -q https://github.com/MissouriMRDT/Autonomy_Packages/raw/main/geolib/am
     dpkg -i geolib_${GEOLIB_VERSION}_amd64.deb && \
     rm geolib_${GEOLIB_VERSION}_amd64.deb
 
-# Install FFMPEG
-ARG FFMPEG_VERSION="7.1"
-RUN wget -q https://github.com/MissouriMRDT/Autonomy_Packages/raw/main/ffmpeg/amd64/ffmpeg_${FFMPEG_VERSION}_amd64.deb && \
-    dpkg -i ffmpeg_${FFMPEG_VERSION}_amd64.deb && \
-    rm ffmpeg_${FFMPEG_VERSION}_amd64.deb
-
 # Install Libdatachannel
 ARG LIBDATACHANNEL_VERSION="0.22"
 RUN wget -q https://github.com/MissouriMRDT/Autonomy_Packages/raw/main/libdatachannel/amd64/libdatachannel_${LIBDATACHANNEL_VERSION}_amd64.deb && \
     dpkg -i libdatachannel_${LIBDATACHANNEL_VERSION}_amd64.deb && \
     rm libdatachannel_${LIBDATACHANNEL_VERSION}_amd64.deb
 
-# Install Tensorflow.
-ARG TENSORFLOW_VERSION="2.15.0"
-RUN wget -q https://github.com/MissouriMRDT/Autonomy_Packages/raw/main/tensorflow/amd64/tensorflow_${TENSORFLOW_VERSION}_amd64.deb && \
-    dpkg -i tensorflow_${TENSORFLOW_VERSION}_amd64.deb && \
-    rm tensorflow_${TENSORFLOW_VERSION}_amd64.deb
+# Install MatPlotPlusPlus
+ARG MATPLOTPLUSPLUS_VERSION="master"
+RUN wget -q https://github.com/MissouriMRDT/Autonomy_Packages/raw/main/matplotplusplus/amd64/matplotplusplus_${MATPLOTPLUSPLUS_VERSION}_amd64.deb && \
+    dpkg -i matplotplusplus_${MATPLOTPLUSPLUS_VERSION}_amd64.deb && \
+    rm matplotplusplus_${MATPLOTPLUSPLUS_VERSION}_amd64.deb
 
 # Install Quill
 ARG QUILL_VERSION="8.1.0"

.devcontainer/JetPack.dockerfile

@@ -36,7 +36,7 @@ RUN apt-get update && apt-get install --no-install-recommends -y \
     libeigen3-dev libglew-dev libgstreamer-plugins-base1.0-dev udev net-tools libssl-dev \
     libgstreamer-plugins-good1.0-dev libgstreamer1.0-dev libgtk-3-dev libjpeg-dev sudo usbutils \
     libjpeg8-dev libjpeg-turbo8-dev liblapack-dev liblapacke-dev libopenblas-dev libpng-dev tzdata \
-    libpostproc-dev libtbb-dev libtbb2 libtesseract-dev libtiff-dev libv4l-dev \
+    libpostproc-dev libtbb-dev libtbb2 libtesseract-dev libtiff-dev libv4l-dev gnuplot-nox \
     libxine2-dev libxvidcore-dev libx264-dev libgtkglext1 libgtkglext1-dev pkg-config qv4l2 \
     v4l-utils zlib1g-dev python3-dev libboost-all-dev valgrind doxygen graphviz nano \
     vim-common libedgetpu1-std gasket-dkms ca-certificates nlohmann-json3-dev curl \
@@ -100,6 +100,18 @@ RUN wget -q https://github.com/MissouriMRDT/Autonomy_Packages/raw/main/pytorch/a
     dpkg -i pytorch_${TORCH_VERSION}_arm64.deb && \
     rm pytorch_${TORCH_VERSION}_arm64.deb
 
+# Install Tensorflow.
+ARG TENSORFLOW_VERSION="2.15.0"
+RUN wget -q https://github.com/MissouriMRDT/Autonomy_Packages/raw/main/tensorflow/arm64/tensorflow_${TENSORFLOW_VERSION}_arm64.deb && \
+    dpkg -i tensorflow_${TENSORFLOW_VERSION}_arm64.deb && \
+    rm tensorflow_${TENSORFLOW_VERSION}_arm64.deb
+
+# Install FFMPEG
+ARG FFMPEG_VERSION="7.1"
+RUN wget -q https://github.com/MissouriMRDT/Autonomy_Packages/raw/main/ffmpeg/arm64/ffmpeg_${FFMPEG_VERSION}_arm64.deb && \
+    dpkg -i ffmpeg_${FFMPEG_VERSION}_arm64.deb && \
+    rm ffmpeg_${FFMPEG_VERSION}_arm64.deb
+
 # Install Abseil.
 ARG ABSEIL_VERSION="20230802.1"
 RUN wget -q https://github.com/MissouriMRDT/Autonomy_Packages/raw/main/abseil/arm64/abseil_${ABSEIL_VERSION}_arm64.deb && \
@@ -112,23 +124,17 @@ RUN wget -q https://github.com/MissouriMRDT/Autonomy_Packages/raw/main/geolib/ar
     dpkg -i geolib_${GEOLIB_VERSION}_arm64.deb && \
     rm geolib_${GEOLIB_VERSION}_arm64.deb
 
-# Install FFMPEG
-ARG FFMPEG_VERSION="7.1"
-RUN wget -q https://github.com/MissouriMRDT/Autonomy_Packages/raw/main/ffmpeg/arm64/ffmpeg_${FFMPEG_VERSION}_arm64.deb && \
-    dpkg -i ffmpeg_${FFMPEG_VERSION}_arm64.deb && \
-    rm ffmpeg_${FFMPEG_VERSION}_arm64.deb
-
 # Install Libdatachannel
 ARG LIBDATACHANNEL_VERSION="0.22"
 RUN wget -q https://github.com/MissouriMRDT/Autonomy_Packages/raw/main/libdatachannel/arm64/libdatachannel_${LIBDATACHANNEL_VERSION}_arm64.deb && \
     dpkg -i libdatachannel_${LIBDATACHANNEL_VERSION}_arm64.deb && \
     rm libdatachannel_${LIBDATACHANNEL_VERSION}_arm64.deb
 
-# Install Tensorflow.
-ARG TENSORFLOW_VERSION="2.15.0"
-RUN wget -q https://github.com/MissouriMRDT/Autonomy_Packages/raw/main/tensorflow/arm64/tensorflow_${TENSORFLOW_VERSION}_arm64.deb && \
-    dpkg -i tensorflow_${TENSORFLOW_VERSION}_arm64.deb && \
-    rm tensorflow_${TENSORFLOW_VERSION}_arm64.deb
+# Install MatPlotPlusPlus
+ARG MATPLOTPLUSPLUS_VERSION="master"
+RUN wget -q https://github.com/MissouriMRDT/Autonomy_Packages/raw/main/matplotplusplus/arm64/matplotplusplus_${MATPLOTPLUSPLUS_VERSION}_arm64.deb && \
+    dpkg -i matplotplusplus_${MATPLOTPLUSPLUS_VERSION}_arm64.deb && \
+    rm matplotplusplus_${MATPLOTPLUSPLUS_VERSION}_arm64.deb
 
 # Install Quill
 ARG QUILL_VERSION="8.1.0"

.devcontainer/devcontainer.json

@@ -15,22 +15,22 @@
         "--privileged",
         "-v",
         "/dev/bus/usb:/dev/bus/usb",
-        // X11 socket for XWayland
-        "-v",
-        "/tmp/.X11-unix:/tmp/.X11-unix",
-        // Wayland socket for native Wayland apps
-        "-v",
-        "/run/user/1000/wayland:/run/user/1000/wayland",
         // WSLg support (if on WSL)
         "-v",
         "/mnt/wslg:/mnt/wslg",
+        // X11 socket for XWayland
+        "-v",
+        "/tmp/.X11-unix:/tmp/.X11-unix",
+        // // Wayland socket for native Wayland apps
+        // "-v",
+        // "/run/user/1000/wayland:/run/user/1000/wayland",
         // Environment variables
-        "-e",
-        "DISPLAY", // XWayland display
-        "-e",
-        "WAYLAND_DISPLAY", // Wayland display, you may need to run 'xhost +local:docker' to allow the container to connect to the X server.
-        "-e",
-        "XDG_RUNTIME_DIR", // Runtime directory for Wayland
+        // "-e",
+        // "DISPLAY", // XWayland display
+        // "-e",
+        // "WAYLAND_DISPLAY", // Wayland display, you may need to run 'xhost +local:docker' to allow the container to connect to the X server.
+        // "-e",
+        // "XDG_RUNTIME_DIR", // Runtime directory for Wayland
         "-e",
         "GDK_BACKEND=x11,wayland" // Allow GTK to work with both backends
     ],
@@ -40,8 +40,8 @@
         "source=${localWorkspaceFolder}/data/calibrations/zed,target=/usr/local/zed/settings,type=bind,consistency=delegated",
         "type=bind,readonly,source=/etc/localtime,target=/etc/localtime"
     ],
-    "image": "ghcr.io/missourimrdt/autonomy-jammy:2025-02-08-23-48-34",
-    // "image": "ghcr.io/missourimrdt/autonomy-jetpack:2025-02-08-23-48-34",
+    "image": "ghcr.io/missourimrdt/autonomy-jammy:2025-02-10-18-03-17",
+    // "image": "ghcr.io/missourimrdt/autonomy-jetpack:2025-02-10-18-03-17",
     // "build": {
     // "dockerfile": "Jammy.dockerfile"
     // "dockerfile": "JetPack.dockerfile"
@@ -72,7 +72,8 @@
                 "streetsidesoftware.code-spell-checker",
                 "vscode-icons-team.vscode-icons",
                 "ryanluker.vscode-coverage-gutters",
-                "GitHub.vscode-pull-request-github"
+                "GitHub.vscode-pull-request-github",
+                "GitHub.copilot"
             ],
             "settings": {
                 // VSCode settings.
@@ -127,6 +128,8 @@
                     "/usr/local/include/libavfilter/**",
                     "/usr/local/include/libavdevice",
                     "/usr/local/include/libavdevice/**",
+                    "/usr/local/include/matplot",
+                    "/usr/local/include/matplot/**",
                     "/usr/local/include/gmock",
                     "/usr/local/include/gmock/**",
                     "/usr/local/include/gtest",
@@ -140,7 +143,9 @@
                     "/usr/include/torch",
                     "/usr/include/torch/**",
                     "/usr/include/nlohmann",
-                    "/usr/include/nlohmann/**"
+                    "/usr/include/nlohmann/**",
+                    "/usr/include/python*",
+                    "/usr/include/python*/**"
                 ],
                 "C_Cpp.default.defines": [
                     "__ROVECOMM_LIBRARY_MODE__=1"

.github/workflows/deploy_docker.yml

@@ -69,6 +69,7 @@ jobs:
     outputs:
       geolib-amd64: ${{ steps.changes.outputs.geolib-amd64 }}
       libdatachannel-amd64: ${{ steps.changes.outputs.libdatachannel-amd64 }}
+      matplotplusplus-amd64: ${{ steps.changes.outputs.matplotplusplus-amd64 }}
       ffmpeg-amd64: ${{ steps.changes.outputs.ffmpeg-amd64 }}
       gtest-amd64: ${{ steps.changes.outputs.gtest-amd64 }}
       opencv-amd64: ${{ steps.changes.outputs.opencv-amd64 }}
@@ -104,6 +105,8 @@ jobs:
               - 'tools/package-builders/geolib/geolib-amd64-pkg.sh'
             libdatachannel-amd64:
               - 'tools/package-builders/libdatachannel/libdatachannel-amd64-pkg.sh'
+            matplotplusplus-amd64:
+              - 'tools/package-builders/matplotplusplus/matplotplusplus-amd64-pkg.sh'
             ffmpeg-amd64:
               - 'tools/package-builders/ffmpeg/ffmpeg-amd64-pkg.sh'
             gtest-amd64:
@@ -153,6 +156,7 @@ jobs:
         package:
           - geolib
           - libdatachannel
+          - matplotplusplus
           - gtest
           - pytorch
           - abseil
@@ -385,6 +389,7 @@ jobs:
     outputs:
       geolib-arm64: ${{ steps.changes.outputs.geolib-arm64 }}
       libdatachannel-arm64: ${{ steps.changes.outputs.libdatachannel-arm64 }}
+      matplotplusplus-arm64: ${{ steps.changes.outputs.matplotplusplus-arm64 }}
       ffmpeg-arm64: ${{ steps.changes.outputs.ffmpeg-arm64 }}
       gtest-arm64: ${{ steps.changes.outputs.gtest-arm64 }}
       opencv-arm64: ${{ steps.changes.outputs.opencv-arm64 }}
@@ -420,6 +425,8 @@ jobs:
               - 'tools/package-builders/geolib/geolib-arm64-pkg.sh'
             libdatachannel-arm64:
               - 'tools/package-builders/libdatachannel/libdatachannel-arm64-pkg.sh'
+            matplotplusplus-arm64:
+              - 'tools/package-builders/matplotplusplus/matplotplusplus-arm64-pkg.sh'
             ffmpeg-arm64:
               - 'tools/package-builders/ffmpeg/ffmpeg-arm64-pkg.sh'
             gtest-arm64:
@@ -468,6 +475,7 @@ jobs:
         package:
           - geolib
           - libdatachannel
+          - matplotplusplus
           - gtest
           - pytorch
           - abseil

.gitignore

@@ -20,6 +20,8 @@ docs/*
 !docs/Doxygen
 docs/Doxygen/*
 !docs/Doxygen/resources
+# Also don't ignore the docs/Whitepapers directory.
+!docs/WhitePapers
 
 # Data. Ignore optimized models, but push base model.
 data/models/zed/.*

CMakeLists.txt

@@ -156,6 +156,9 @@ include_directories(external/rovecomm/src)
 add_subdirectory(external/rovecomm)
 add_compile_definitions(__ROVECOMM_LIBRARY_MODE__=1)
 
+## Find Python3
+find_package(Python3 COMPONENTS Development NumPy REQUIRED)
+
 ## Determine if shared or static libraries will be used.
 if (NOT LINK_SHARED_ZED AND MSVC)
     message(FATAL_ERROR "LINK_SHARED_ZED OFF : ZED SDK static libraries not available on Windows")
@@ -184,6 +187,9 @@ find_package(PkgConfig REQUIRED)
 pkg_check_modules(FFMPEG REQUIRED IMPORTED_TARGET libavcodec libavformat libavutil libswscale libswresample libavdevice libavfilter libpostproc SvtAv1Enc)
 include_directories(${FFMPEG_INCLUDE_DIRS})
 
+## Find MatPlotPlusPlus
+find_package(Matplot++ REQUIRED)
+
 ## Find Google Test
 find_package(GTest CONFIG REQUIRED)
 include(GoogleTest)
@@ -290,11 +296,13 @@ set(AUTONOMY_LIBRARIES  Threads::Threads
                         OpenMP::OpenMP_CXX
                         Eigen3::Eigen
                         RoveComm_CPP
+                        Python3::Python
                         quill::quill
                         ${OPENSSL_LIBRARIES}
                         PkgConfig::FFMPEG
                         LibDataChannel::LibDataChannel
                         nlohmann_json::nlohmann_json
+                        Matplot++::matplot
                         ${OpenCV_LIBS}
                         ${GeographicLib_LIBRARIES}
                         ${ZED_LIBS}

CONTRIBUTING.md

@@ -185,6 +185,7 @@ External libraries and their corresponding type prefixes are listed below:
 | WebRTC         | `rtc`  | `rtc::PeerConnection rtcExampleRTCType` |
 | JSON           | `jsn`  | `nlohmann::json jsnExampleJSONType` |
 | FFMPEG         | `av`   | `AVPacket avExampleFFMPEGPacket` |
+| Matplot        | `mt`   | `axes_handle mtExampleMatplot`   |
 
 ---
 

README.md

@@ -331,6 +331,10 @@ This project makes use of several open-source libraries and tools that help us a
     [nlohmann JSON GitHub](https://github.com/nlohmann/json)  
     License: [MIT License](https://github.com/nlohmann/json/blob/develop/LICENSE.MIT)
 
+- **matplotplusplus** - Matplot++: A C++ Graphics Library for Data Visualization
+    [Matplot++](https://github.com/alandefreitas/matplotplusplus)
+    License: [MIT License](https://github.com/alandefreitas/matplotplusplus/blob/master/LICENSE)
+
 - **Abseil** - Collection of C++ libraries designed for performance and ease of use.  
   [Abseil GitHub](https://github.com/abseil/abseil-cpp)  
   License: [Apache License 2.0](https://github.com/abseil/abseil-cpp/blob/master/LICENSE)

data/Custom_Dictionaries/Autonomy-Dictionary.txt

@@ -8,6 +8,7 @@ ADDMARKERLEG
 ADDOBJECTLEG
 ADDOBSTACLE
 ADDPOSITIONLEG
+alandefreitas
 algobase
 alloc
 allocs
@@ -369,7 +370,10 @@ MAKEFLAGS
 MAPANGLE
 MAPRANGE
 marsrover
+MATPLOT
 matplotlib
+matplotlibcpp
+matplotplusplus
 matx
 Memcheck
 MERICA
@@ -542,10 +546,12 @@ Wpedantic
 wslg
 xauth
 xhost
+xlabel
 XNNPACK
 XYZBGRA
 XYZRGBA
 yasm
+ylabel
 YOLO
 yolomodel
 YUVJ
@@ -554,4 +560,5 @@ Zain
 ZEDCAM
 ZEDCAMERA
 ZEDSDK
+zlabel
 zstd

src/AutonomyConstants.h

@@ -40,7 +40,8 @@ namespace constants
 #else
     const bool MODE_SIM = false;    // REG MODE ENABLED: Toggle RoveComm and Cameras to use standard configuration.
 #endif
-    const std::string SIM_IP_ADDRESS = "192.168.69.29";    // The IP address to use for simulation mode.
+    const std::string SIM_IP_ADDRESS = "192.168.2.118";    // The IP address to use for simulation mode.
+    const uint SIM_WEBSOCKET_PORT    = 8080;               // The port to use for the WebSocket in simulation mode.
     const uint SIM_WEBRTC_QP         = 25;    // The QP value to use for WebRTC in simulation mode. 0-51, 0 is lossless. If too high for network, frames drop.
 
     // Safety constants.
@@ -350,13 +351,15 @@ namespace constants
     ///////////////////////////////////////////////////////////////////////////
 
     // Stanley Controller config.
-    const double STANLEY_STEER_CONTROL_GAIN = 0.5;    // Determines how reactive the rover is to yaw adjustments.
-    const double STANLEY_DIST_TO_FRONT_AXLE = 2.9;    // Distance from position sensor to the center of the front axle.
-    const double STANLEY_YAW_TOLERANCE      = 1.0;    // Threshold for limiting unnecessary small movements.
+    const double STANLEY_CROSSTRACK_CONTROL_GAIN = 2.0;    // Determines how reactive the rover is to crosstrack error adjustments. This is in degrees scale to the error.
+    const double STANLEY_DIST_TO_FRONT_AXLE      = 1.0;    // Distance from position sensor to the center of the front axle in meters.
+    const double STANLEY_STEERING_ANGLE_LIMIT    = 100.0;    // The maximum steering angle in degrees.
+    const int STANLEY_PREDICTION_HORIZON         = 5;        // The number of predictions to make.
+    const double STANLEY_PREDICTION_TIME_STEP    = 0.5;      // The time to pass in seconds between each prediction of the Stanley controller bicycle model.
 
     // ASTAR config.
     const double ASTAR_AVOIDANCE_MULTIPLIER = 1.2;          // Multiplier for marking extra nodes around objects as obstacles
-    const double ASTAR_MAXIMUM_SEARCH_GRID  = 10.0;         // Maximum search grid size (UTM)
+    const double ASTAR_MAXIMUM_SEARCH_GRID  = 1000.0;       // Maximum search grid size (UTM)
     const double ASTAR_NODE_SIZE            = 0.5;          // Represents the node size / accuracy in meters
     const double ASTAR_SQRT_NODE_SIZE       = M_SQRT1_2;    // Square root of m_dNodeSize
 

src/AutonomyLogging.cpp

@@ -9,6 +9,7 @@
  ******************************************************************************/
 
 #include "AutonomyLogging.h"
+#include "./util/logging/PlotsAndGraphs.hpp"
 #include "AutonomyNetworking.h"
 
 /// \cond
@@ -44,6 +45,7 @@ namespace logging
     quill::LogLevel g_eRoveCommLogLevel;
 
     std::string g_szProgramStartTimeString;
+    std::string g_szLoggingOutputPath;
 
     /******************************************************************************
      * @brief Logger Initializer - Sets Up all the logging handlers required for
@@ -67,6 +69,9 @@ namespace logging
         szFilePath += g_szProgramStartTimeString + "/";    // Folder for each program run.
         szFilename = "console_output";                     // Base file name.
 
+        // Store the logging output path.
+        g_szLoggingOutputPath = szFilePath;
+
         // Check if directory exists.
         if (!std::filesystem::exists(szFilePath))
         {