Write Drivers for Rover Hardware Interfaces

.devcontainer/Dockerfile_Focal

@@ -16,6 +16,7 @@ ARG ZED_MAJOR="4"
 ARG ZED_MINOR="0"
 ARG CMAKE_VERSION="3.24.3"
 ARG OPENCV_VERSION="4.8.0"
+ARG GEOLIB_VERSION="v2.3"
 ARG QUILL_VERSION="v3.3.1"
 ARG GTEST_VERSION="v1.14.0"
 ARG TZ="America/Chicago"
@@ -113,11 +114,23 @@ RUN git clone --depth 1 --branch ${OPENCV_VERSION} https://github.com/opencv/ope
     cd ../.. && \
     rm -rf opencv_contrib && rm -rf opencv
 
+# Install GeographicLib from Source
+RUN git clone --depth 1 --branch ${GEOLIB_VERSION} https://github.com/geographiclib/geographiclib.git && \
+    mkdir geographiclib/build && \
+    cd geographiclib/build && \
+    cmake \
+    -D CMAKE_BUILD_TYPE=Release .. && \
+    make && \
+    make install && \
+    cd ../.. && \
+    rm -rf geographiclib
+
 # Install Quill from Source
 RUN git clone --depth 1 --branch ${QUILL_VERSION} http://github.com/odygrd/quill.git && \
     mkdir quill/build && \
     cd quill/build && \
-    cmake .. && \
+    cmake \ 
+    -D CMAKE_BUILD_TYPE=Release .. && \
     make && \
     make install && \
     cd ../.. && \

.devcontainer/Dockerfile_Jammy

@@ -16,6 +16,7 @@ ARG ZED_MAJOR="4"
 ARG ZED_MINOR="0"
 ARG CMAKE_VERSION="3.24.3"
 ARG OPENCV_VERSION="4.8.0"
+ARG GEOLIB_VERSION="v2.3"
 ARG QUILL_VERSION="v3.3.1"
 ARG GTEST_VERSION="v1.14.0"
 ARG TZ="America/Chicago"
@@ -116,6 +117,17 @@ RUN git clone --depth 1 --branch ${OPENCV_VERSION} https://github.com/opencv/ope
     cd ../.. && \
     rm -rf opencv_contrib && rm -rf opencv
 
+# Install GeographicLib from Source
+RUN git clone --depth 1 --branch ${GEOLIB_VERSION} https://github.com/geographiclib/geographiclib.git && \
+    mkdir geographiclib/build && \
+    cd geographiclib/build && \
+    cmake \
+    -D CMAKE_BUILD_TYPE=RelWithDebInfo .. && \
+    make && \
+    make install && \
+    cd ../.. && \
+    rm -rf geographiclib
+
 # Install Quill from Source
 RUN git clone --depth 1 --branch ${QUILL_VERSION} http://github.com/odygrd/quill.git && \
     mkdir quill/build && \

.devcontainer/Dockerfile_JetPack

@@ -16,6 +16,7 @@ ARG ZED_MAJOR="4"
 ARG ZED_MINOR="0"
 ARG CMAKE_VERSION="3.24.3"
 ARG OPENCV_VERSION="4.8.0"
+ARG GEOLIB_VERSION="v2.3"
 ARG QUILL_VERSION="v3.3.1"
 ARG GTEST_VERSION="v1.14.0"
 ARG TZ="America/Chicago"
@@ -115,6 +116,17 @@ RUN git clone --depth 1 --branch ${OPENCV_VERSION} https://github.com/opencv/ope
     cd ../.. && \
     rm -rf opencv_contrib && rm -rf opencv
 
+# Install GeographicLib from Source
+RUN git clone --depth 1 --branch ${GEOLIB_VERSION} https://github.com/geographiclib/geographiclib.git && \
+    mkdir geographiclib/build && \
+    cd geographiclib/build && \
+    cmake \
+    -D CMAKE_BUILD_TYPE=RelWithDebInfo .. && \
+    make && \
+    make install && \
+    cd ../.. && \
+    rm -rf geographiclib
+
 # Install Quill from Source
 RUN git clone --depth 1 --branch ${QUILL_VERSION} http://github.com/odygrd/quill.git && \
     mkdir quill/build && \

.devcontainer/devcontainer.json

@@ -32,7 +32,7 @@
     // "image": "ghcr.io/missourimrdt/autonomy-jetpack:latest",
     // "build": {
     //     // "dockerfile": "Dockerfile_Jammy"
-    //     "dockerfile": "Dockerfile_Focal"
+    //     // "dockerfile": "Dockerfile_Focal"
     //     // "dockerfile": "Dockerfile_JetPack"
     // },
     // Features to add to the dev container. More info: https://containers.dev/features.
@@ -95,7 +95,9 @@
                     "/usr/local/include/gmock/",
                     "/usr/local/include/gmock/**",
                     "/usr/local/include/gtest/",
-                    "/usr/local/include/gtest/**"
+                    "/usr/local/include/gtest/**",
+                    "/usr/local/include/GeographicLib/",
+                    "/usr/local/include/GeographicLib/**"
                 ],
                 // CMAKE extension settings.
                 "cmake.configureOnOpen": true,

CMakeLists.txt

@@ -74,6 +74,10 @@ find_package(Eigen3 3.3 REQUIRED NO_MODULE)
 find_package(OpenCV REQUIRED)
 include_directories(${OpenCV_INCLUDE_DIRS})
 
+## Find Geographic Lib.
+find_package(GeographicLib REQUIRED)
+include_directories(${GeographicLib_INCLUDE_DIRS})
+
 ## Find CUDA.
 find_package(CUDA 11.4 REQUIRED)
 message(STATUS "Found CUDA ${CUDA_VERSION_STRING} at ${CUDA_TOOLKIT_ROOT_DIR}")
@@ -133,12 +137,13 @@ target_link_libraries(${PROJECT_NAME} PRIVATE   Threads::Threads
 												Eigen3::Eigen
                                                 quill::quill
 												${OpenCV_LIBS}
+                                                ${GeographicLib_LIBRARIES}
                                                 ${ZED_LIBS} 
                                                 RoveComm_CPP
     )
 
 ## Package Executable
-install(TARGETS ${PROJECT_NAME} RUNTIME_DEPENDENCIES DIRECTORIES ${OpenCV_LIBS} ${ZED_LIBS} quill::quill RoveComm_CPP RUNTIME DESTINATION bin)
+install(TARGETS ${PROJECT_NAME} RUNTIME_DEPENDENCIES DIRECTORIES ${OpenCV_LIBS} ${GeographicLib_LIBRARIES} ${ZED_LIBS} quill::quill RoveComm_CPP RUNTIME DESTINATION bin)
 
 ## Unit/Integration Tests
 file(GLOB_RECURSE UnitTests_SRC         CONFIGURE_DEPENDS  "tests/Unit/*.cc")
@@ -150,15 +155,15 @@ list(LENGTH IntegrationTests_SRC    IntegrationTests_LEN)
 
 if (UnitTests_LEN GREATER 0)
     add_executable(${PROJECT_NAME}_UnitTests ${UnitTests_SRC} ${Logging_SRC})
-    target_link_libraries(${PROJECT_NAME}_UnitTests GTest::gtest GTest::gtest_main ${OpenCV_LIBS} quill::quill)
+    target_link_libraries(${PROJECT_NAME}_UnitTests GTest::gtest GTest::gtest_main quill::quill ${OpenCV_LIBS} ${GeographicLib_LIBRARIES})
     add_test(Unit_Tests ${PROJECT_NAME}_UnitTests)
 else()
     message("No Unit Tests!")
 endif()
 
 if (IntegrationTests_LEN GREATER 0)
     add_executable(${PROJECT_NAME}_IntegrationTests ${IntegrationTests_SRC} ${Logging_SRC})
-    target_link_libraries(${PROJECT_NAME}_IntegrationTests GTest::gtest GTest::gtest_main quill::quill)
+    target_link_libraries(${PROJECT_NAME}_IntegrationTests GTest::gtest GTest::gtest_main quill::quill ${OpenCV_LIBS} ${GeographicLib_LIBRARIES})
     add_test(Integration_Tests ${PROJECT_NAME}_IntegrationTests)
 else()
     message("No Integration Tests!")

CONTRIBUTING.md

@@ -103,6 +103,7 @@ Examples: `int g_nExampleGlobalInteger` or `int m_nExampleMemberInteger`
 - Boost:
     - State Chart > `sc`  > Example: `sc::simple_state<IdleState, StateMachine>`
 - Quill  > `q` > Example: `quill::Config qConfig`
+- GeographicLib > `ge` > Example: `GeographicLib::Geodesic geExampleGeographicType`
 
 ### Macro Names
 Macros should be in all caps using underscores to separate words. They should be detailed enough to not require an additional comment.

data/Custom_Dictionaries/Autonomy-Dictionary.txt

@@ -4,28 +4,42 @@ alloc
 allocs
 APRILTAG
 Aruco
+autofetch
 arucotag
 AUTONOMYTHREAD
 BASICCAM
 CALIB
 clayjay
 codeql
+codezombiech
 Coeffs
+Colour
+cpptools
+cschlosser
 CUDA
 deadband
 deque
 Desaturate
 devcontainer
 diffdrive
+doxdocgen
 Doxygen
 driveboard
+Extention
+feedforward
 flightsticks
 geoops
 Geospatial
+gmock
+GPSMDRS
+GPSSDELC
+gruntfuggly
+gtest
 hyperthreaded
 imgops
 imread
 imshow
+jbockle
 inferencing
 jetpack
 jspencerpittman
@@ -37,6 +51,7 @@ mainpage
 MAPRANGE
 Memcheck
 microcontrollers
+missourimrdt
 MJPEG
 MRDT
 MULTIMEDIABOARD
@@ -55,9 +70,12 @@ reprojection
 RESOLUTIONX
 RESOLUTIONY
 RGBE
+Rolla
 RIGHTCAM
 rovecomm
 searchbar
+setpoint
+setpoints
 SIMCAM
 statechart
 Struct
@@ -67,13 +85,18 @@ Tele
 tensorflowtag
 threadpool
 tparam
+twxs
 UART
+UTMMDRS
+UTMUPS
 UNVALIDATED
 valgrind
 Watchpoints
+wslg
 XYZBGRA
 XYZRGBA
 YUVJ
 YUYV
+Zain
 ZEDCAM
 ZEDSDK

src/AutonomyConstants.h

@@ -37,7 +37,16 @@ namespace constants
     const float DRIVE_MIN_EFFORT = -0.5;
 
     // Control constants.
-    const bool DRIVE_SQUARE_CONTROL_INPUTS = false;    // This is used by the DifferentialDrive algorithms. True makes fine inputs smoother, but less responsive.
+    const double DRIVE_PID_PROPORTIONAL       = 0.1;      // The proportional gain for the controller used to point the rover at a goal heading during navigation.
+    const double DRIVE_PID_INTEGRAL           = 0.01;     // The integral gain for the controller used to point the rover at a goal heading during navigation.
+    const double DRIVE_PID_DERIVATIVE         = 0.0;      // The derivative gain for the controller used to point the rover at a goal heading during navigation.
+    const double DRIVE_PID_MAX_ERROR_PER_ITER = 100;      // The max allowable error the controller will see per iteration. This is on degrees from setpoint.
+    const double DRIVE_PID_MAX_INTEGRAL_TERM  = 0.3;      // The max effort the I term is allowed to contribute.
+    const double DRIVE_PID_MAX_OUTPUT_EFFORT  = 0.5;      // The max effort the entire PID controller is allowed to output. Range is within DRIVE_MAX/MIN_POWER.
+    const double DRIVE_PID_MAX_RAMP_RATE      = 0.4;      // The max ramp rate of the output of the PID controller.
+    const double DRIVE_PID_OUTPUT_FILTER      = 0.0;      // Larger values will filter out large spikes or oscillations. 0.1 is a good starting point.
+    const bool DRIVE_PID_OUTPUT_REVERSED      = false;    // Negates the output of the PID controller.
+    const bool DRIVE_SQUARE_CONTROL_INPUTS    = false;    // This is used by the DifferentialDrive algorithms. True makes fine inputs smoother, but less responsive.
     const bool DRIVE_CURVATURE_KINEMATICS_ALLOW_TURN_WHILE_STOPPED = true;    // This enabled turning in-place when using curvature drive control.
     ///////////////////////////////////////////////////////////////////////////