Merge pull request #207 from MissouriMRDT/feature/sim-cpp-integration

Integrate Autonomy with RoveComm and SIM
MissouriMRDT · Mar 18, 2024 · fe453b9 · fe453b9
2 parents a7610ee + 2d0190c
commit fe453b9
Show file tree

Hide file tree

Showing 42 changed files with 1,150 additions and 542 deletions.
diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json
@@ -4,6 +4,9 @@
         // Name container.
         "--name",
         "Autonomy_Software_devcontainer",
+        // Configure container to use the same network stack as the host machine.
+        "--network",
+        "host",
         // Add all available GPUs
         "--gpus",
         "all",

diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -28,8 +28,20 @@ if(NOT CTest_CMake_INCLUDED)
 endif()
 include(FetchContent)                                                               # CMake Dependency Framework
 
+## Enable or Disble Simulation Mode
+option(BUILD_SIM_MODE "Enable Simulation Mode" OFF)
+if(BUILD_SIM_MODE)
+    message("Sim Mode: Enabled")
+    add_compile_definitions(__AUTONOMY_SIM_MODE__=1)
+    set(EXE_NAME "${PROJECT_NAME}_Sim")
+else()
+    message("Sim Mode: Disabled")
+    add_compile_definitions(__AUTONOMY_SIM_MODE__=0)
+    set(EXE_NAME "${PROJECT_NAME}")
+endif()
+
 ## Define Project Name and Version Number for CPack
-set(CPACK_PROJECT_NAME ${PROJECT_NAME})
+set(CPACK_PROJECT_NAME ${EXE_NAME})
 set(CPACK_PROJECT_VERSION ${PROJECT_VERSION})
 set(CPACK_PACKAGE_VENDOR "Mars Rover Design Team")
 set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE")
@@ -40,7 +52,7 @@ set(CPACK_GENERATOR "STGZ")
 ## Find RoveComm
 FetchContent_Declare(RoveComm_CPP                                                   # Custom Network Protocol (RoveComm C++)
         GIT_REPOSITORY https://github.com/MissouriMRDT/RoveComm_CPP.git             # Source Code URL
-        GIT_TAG        52140ebaa75c889c4e041645ba23ba4accfec8e8                     # Version 24.2.0
+        GIT_TAG        bdcb60bcea4658f6272289db069efd133f133dec                     # Version 24.2.2
         GIT_SHALLOW    FALSE                                                        # Download only without history
         GIT_PROGRESS   TRUE                                                         # Forces progress status.
     )
@@ -122,21 +134,21 @@ file(GLOB_RECURSE External_SRC		  	CONFIGURE_DEPENDS  "external/src/*.cpp")
 file(GLOB_RECURSE Tools_SRC			 	CONFIGURE_DEPENDS  "tools/*.cpp")
 
 ## Create Executable File
-add_executable(${PROJECT_NAME}   ${External_SRC}
+add_executable(${EXE_NAME}   ${External_SRC}
 								 ${SRC}
 								 ${Examples_SRC}
 								 ${Tools_SRC}
 	)
 
 ## Set Compile Options for Autonomy Software.
 if(MSVC) # True when compiler is Microsoft Visual C++/simulation of Visual C++ CL.
-	target_compile_options(${PROJECT_NAME} PRIVATE /W4 /WX)
+	target_compile_options(${EXE_NAME} PRIVATE /W4 /WX)
 else()
-	target_compile_options(${PROJECT_NAME} PRIVATE -Wall -Wextra -Wpedantic)
+	target_compile_options(${EXE_NAME} PRIVATE -Wall -Wextra -Wpedantic)
 endif()
 
 ## Link Libraries to Executable
-target_link_libraries(${PROJECT_NAME} PRIVATE   Threads::Threads
+target_link_libraries(${EXE_NAME} PRIVATE   Threads::Threads
 												Eigen3::Eigen
                                                 quill::quill
 												${OpenCV_LIBS}
@@ -146,7 +158,7 @@ target_link_libraries(${PROJECT_NAME} PRIVATE   Threads::Threads
     )
 
 ## Package Executable
-install(TARGETS ${PROJECT_NAME} RUNTIME_DEPENDENCIES DIRECTORIES ${OpenCV_LIBS} ${GeographicLib_LIBRARIES} ${ZED_LIBS} quill::quill RoveComm_CPP RUNTIME DESTINATION bin)
+install(TARGETS ${EXE_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")
@@ -158,17 +170,17 @@ list(LENGTH UnitTests_SRC           UnitTests_LEN)
 list(LENGTH IntegrationTests_SRC    IntegrationTests_LEN)
 
 if (UnitTests_LEN GREATER 0)
-    add_executable(${PROJECT_NAME}_UnitTests ${UnitTests_SRC} ${Algorithms_SRC} ${Logging_SRC})
-    target_link_libraries(${PROJECT_NAME}_UnitTests GTest::gtest GTest::gtest_main quill::quill ${OpenCV_LIBS} ${GeographicLib_LIBRARIES})
-    add_test(Unit_Tests ${PROJECT_NAME}_UnitTests)
+    add_executable(${EXE_NAME}_UnitTests ${UnitTests_SRC} ${Algorithms_SRC} ${Logging_SRC})
+    target_link_libraries(${EXE_NAME}_UnitTests GTest::gtest GTest::gtest_main quill::quill ${OpenCV_LIBS} ${GeographicLib_LIBRARIES})
+    add_test(Unit_Tests ${EXE_NAME}_UnitTests)
 else()
     message("No Unit Tests!")
 endif()
 
 if (IntegrationTests_LEN GREATER 0)
-    add_executable(${PROJECT_NAME}_IntegrationTests ${IntegrationTests_SRC} ${Algorithms_SRC} ${Logging_SRC})
-    target_link_libraries(${PROJECT_NAME}_IntegrationTests GTest::gtest GTest::gtest_main quill::quill ${OpenCV_LIBS} ${GeographicLib_LIBRARIES})
-    add_test(Integration_Tests ${PROJECT_NAME}_IntegrationTests)
+    add_executable(${EXE_NAME}_IntegrationTests ${IntegrationTests_SRC} ${Algorithms_SRC} ${Logging_SRC})
+    target_link_libraries(${EXE_NAME}_IntegrationTests GTest::gtest GTest::gtest_main quill::quill ${OpenCV_LIBS} ${GeographicLib_LIBRARIES})
+    add_test(Integration_Tests ${EXE_NAME}_IntegrationTests)
 else()
     message("No Integration Tests!")
 endif()
diff --git a/README.md b/README.md
@@ -82,3 +82,27 @@ The **src** directory serves as the main source code directory and contains the
 - See [INSTALL.md](INSTALL.md) for full installation instructions.
 - See [CONTRIBUTING.md](CONTRIBUTING.md) for full contribution instructions.
 - See [DEBUG.md](DEBUG.md) for full debugging and development instructions.
+
+# Running Autonomy
+
+There are two primary run modes for our Autonomy Codebase.
+
+1) **Standard Mode:** Which operates using the Onboard Rover System and communicates using the standard RoveComm manifest and IP Addresses. To build and run Autonomy in this mode use the following commands:
+
+    ```
+    mkdir -p <Autonomy Install Location>/build
+    cd <Autonomy Install Location>/build
+    cmake -DBUILD_SIM_MODE=OFF ..
+    make
+    ./Autonomy_Software
+    ```
+
+2) **Simulation Mode:** Which operates using the Webots Robot Simulator and uses the RoveComm manifest but sends all packets to local host instead. This allows the robot simulator to retrieve all the packets and run the simulation. To build and run Autonomy in this mode use the following commands:
+
+    ```
+    mkdir -p <Autonomy Install Location>/build
+    cd <Autonomy Install Location>/build
+    cmake -DBUILD_SIM_MODE=ON ..
+    make
+    ./Autonomy_Software_Sim
+    ```
diff --git a/data/Custom_Dictionaries/Autonomy-Dictionary.txt b/data/Custom_Dictionaries/Autonomy-Dictionary.txt
@@ -1,5 +1,9 @@
 # Custom Dictionary Words
 ABGR
+ACCURACYDATA
+ADDMARKERLEG
+ADDOBJECTLEG
+ADDPOSITIONLEG
 alloc
 allocs
 APPROACHINGMARKERSTATE
@@ -16,20 +20,24 @@ buildcache
 Buildx
 CALIB
 clayjay
+CLEARWAYPOINTS
 codeql
 codezombiech
 Coeffs
 Colour
+COMPASSDATA
 Contoller
 cpptools
 cschlosser
 CUDA
+CURRENTSTATE
 deadband
 debfile
 deque
 Desaturate
 devcontainer
 diffdrive
+DISABLEAUTONOMY
 DISPLAYSTATE
 dockerfiles
 dorny
@@ -48,6 +56,7 @@ geoops
 Geospatial
 gmock
 GNSS
+GPSLATLONALT
 GPSMDRS
 GPSSDELC
 gruntfuggly
@@ -101,6 +110,7 @@ rovecomm
 searchbar
 searchpattern
 SEARCHPATTERNSTATE
+SETMAXSPEED
 setpoint
 setpoints
 SIMCAM
@@ -110,6 +120,7 @@ statechart
 STATEDISPLAY
 statemachine
 STATEMACHINEHANDLER
+STOPAUTONOMY
 Struct
 STUCKSTATE
 Styleguides
@@ -131,6 +142,7 @@ VERIFYINGMARKERSTATE
 VERIFYINGOBJECTSTATE
 VIDEOWRITER
 Watchpoints
+Webots
 wslg
 XYZBGRA
 XYZRGBA

diff --git a/src/AutonomyConstants.h b/src/AutonomyConstants.h
@@ -30,21 +30,23 @@
 namespace constants
 {
     ///////////////////////////////////////////////////////////////////////////
-    //// Logging Constants.
+    //// General Constants.
     ///////////////////////////////////////////////////////////////////////////
 
-    // Output Paths.
-    const std::string LOGGING_OUTPUT_PATH_ABSOLUTE = "./logs/";    // The absolute to write output logging and video files to.
-    ///////////////////////////////////////////////////////////////////////////
+    // Program mode constants.
+#if defined(__AUTONOMY_SIM_MODE__) && __AUTONOMY_SIM_MODE__ == 1
+    const bool MODE_SIM = true;    // SIM MODE ENABLED: Toggle RoveComm and Cameras to use local data from the Webots SIM.
+#else
+    const bool MODE_SIM = false;    // REG MODE ENABLED: Toggle RoveComm and Cameras to use standard configuration.
+#endif
 
-    ///////////////////////////////////////////////////////////////////////////
-    //// RoveComm Constants.
-    ///////////////////////////////////////////////////////////////////////////
+    // Logging constants.
+    const std::string LOGGING_OUTPUT_PATH_ABSOLUTE = "./logs/";    // The absolute to write output logging and video files to.
 
-    // Socket Ports.
-    const int ROVECOMM_UDP_PORT                 = 11000;    // The UDP socket port to use for the main UDP RoveComm instance.
-    const int ROVECOMM_TCP_PORT                 = 12000;    // The UDP socket port to use for the main UDP RoveComm instance.
-    const std::string ROVECOMM_TCP_INTERFACE_IP = "";       // The IP address to bind the socket to. If set to "", the socket will be bound to all available interfaces.
+    // RoveComm constants.
+    const int ROVECOMM_OUTGOING_UDP_PORT        = MODE_SIM ? 11001 : 11000;    // The UDP socket port to use for the main UDP RoveComm instance.
+    const int ROVECOMM_OUTGOING_TCP_PORT        = MODE_SIM ? 12001 : 12000;    // The UDP socket port to use for the main UDP RoveComm instance.
+    const std::string ROVECOMM_TCP_INTERFACE_IP = "";    // The IP address to bind the socket to. If set to "", the socket will be bound to all available interfaces.
     ///////////////////////////////////////////////////////////////////////////
 
     ///////////////////////////////////////////////////////////////////////////
@@ -54,20 +56,19 @@ namespace constants
     // Power constants.
     const float DRIVE_MAX_POWER  = 1.0;
     const float DRIVE_MIN_POWER  = -1.0;
-    const float DRIVE_MAX_EFFORT = 0.5;
-    const float DRIVE_MIN_EFFORT = -0.5;
+    const float DRIVE_MAX_EFFORT = 1.0;
+    const float DRIVE_MIN_EFFORT = -1.0;
 
     // Control constants.
-    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 = DRIVE_MAX_EFFORT;    // 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 double DRIVE_PID_PROPORTIONAL       = 0.01;     // The proportional gain for the controller used to point the rover at a goal heading during navigation.
+    const double DRIVE_PID_INTEGRAL           = 0.0;      // 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 = 180;      // 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_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.
     ///////////////////////////////////////////////////////////////////////////
 
@@ -222,7 +223,8 @@ namespace constants
     //// State Constants.
     ///////////////////////////////////////////////////////////////////////////
 
-    // Nothing here yet.
+    // Navigating State.
+    const double NAVIGATING_REACHED_GOAL_RADIUS = 1.0;    // The radius in meters that the rover should get to the goal waypoint.
     ///////////////////////////////////////////////////////////////////////////
 
     ///////////////////////////////////////////////////////////////////////////

diff --git a/src/AutonomyLogging.cpp b/src/AutonomyLogging.cpp
@@ -86,13 +86,13 @@ namespace logging
         std::shared_ptr<quill::Handler> qConsoleHandler = quill::stdout_handler();
 
         // Configure Patterns
-        qFileHandler->set_pattern("%(ascii_time) %(level_name) [%(process)] [%(thread)] %(message)",       // format
-                                  "%Y-%m-%d %H:%M:%S.%Qms",                                                // timestamp format
-                                  quill::Timezone::GmtTime);                                               // timestamp's timezone
+        qFileHandler->set_pattern("%(ascii_time) %(level_name) [%(thread)] [%(filename):%(lineno)] %(message)",       // format
+                                  "%Y-%m-%d %H:%M:%S.%Qms",                                                           // timestamp format
+                                  quill::Timezone::GmtTime);                                                          // timestamp's timezone
 
-        qConsoleHandler->set_pattern("%(ascii_time) %(level_name) [%(process)] [%(thread)] %(message)",    // format
-                                     "%Y-%m-%d %H:%M:%S.%Qms",                                             // timestamp format
-                                     quill::Timezone::GmtTime);                                            // timestamp's timezone
+        qConsoleHandler->set_pattern("%(ascii_time) %(level_name) [%(thread)] [%(filename):%(lineno)] %(message)",    // format
+                                     "%Y-%m-%d %H:%M:%S.%Qms",                                                        // timestamp format
+                                     quill::Timezone::GmtTime);                                                       // timestamp's timezone
 
         // Enable Color Console
         static_cast<quill::ConsoleHandler*>(qConsoleHandler.get())->enable_console_colours();
@@ -106,17 +106,21 @@ namespace logging
         // Start Quill
         quill::start();
 
+        // Set Handler Filters
+        qFileHandler->add_filter(std::make_unique<LoggingFilter>("FileFilter", quill::LogLevel::TraceL3));
+        qConsoleHandler->add_filter(std::make_unique<LoggingFilter>("ConsoleFilter", quill::LogLevel::Info));
+
         // Create Loggers
         g_qFileLogger    = quill::create_logger("FILE_LOGGER", {qFileHandler});
         g_qConsoleLogger = quill::create_logger("CONSOLE_LOGGER", {qConsoleHandler});
         g_qSharedLogger  = quill::create_logger("SHARED_LOGGER", {qFileHandler, qConsoleHandler});
 
-        // Set Logging Levels
+        // Set Base Logging Levels
+        g_qSharedLogger->set_log_level(quill::LogLevel::TraceL3);
         g_qFileLogger->set_log_level(quill::LogLevel::TraceL3);
         g_qConsoleLogger->set_log_level(quill::LogLevel::TraceL3);
-        g_qSharedLogger->set_log_level(quill::LogLevel::TraceL3);
 
-        // // Enable Backtrace
+        // Enable Backtrace
         g_qFileLogger->init_backtrace(2, quill::LogLevel::Critical);
         g_qConsoleLogger->init_backtrace(2, quill::LogLevel::Critical);
         g_qSharedLogger->init_backtrace(2, quill::LogLevel::Critical);

diff --git a/src/AutonomyLogging.h b/src/AutonomyLogging.h
@@ -63,5 +63,64 @@ namespace logging
     /////////////////////////////////////////
 
     void InitializeLoggers(std::string szLoggingOutputPath);
+
+    /////////////////////////////////////////
+    // Define namespace file filters.
+    /////////////////////////////////////////
+
+    /******************************************************************************
+     * @brief This class serves as a container class for handling log filtering of
+     *    loggers. This must be used if you want each handler to have a different
+     *    logging level since adding multiple handlers to the same logger will apply the
+     *    loggers logging level to each handler.
+     *
+     *
+     * @author clayjay3 ([email protected])
+     * @date 2024-03-16
+     ******************************************************************************/
+    class LoggingFilter : public quill::FilterBase
+    {
+        private:
+            // Declare private member variables.
+            quill::LogLevel m_eMinLogLevel;
+
+        public:
+            /******************************************************************************
+             * @brief Construct a new Console Filter object.
+             *
+             * @param eMinLogLevel - The minimum acceptable log level for the console handler.
+             *      All log levels above this will also be logged.
+             *
+             * @author clayjay3 ([email protected])
+             * @date 2024-03-16
+             ******************************************************************************/
+            LoggingFilter(const std::string szFilterBaseType, const quill::LogLevel eMinLogLevel) : quill::FilterBase(szFilterBaseType)
+            {
+                // Set member variables.
+                m_eMinLogLevel = eMinLogLevel;
+            };
+
+            /******************************************************************************
+             * @brief This method should never be called by this codebase, it is called internally
+             *      by the quill library.
+             *
+             * @author clayjay3 ([email protected])
+             * @date 2024-03-16
+             ******************************************************************************/
+            QUILL_NODISCARD bool filter(char const* thread_id,
+                                        std::chrono::nanoseconds log_message_timestamp,
+                                        quill::MacroMetadata const& metadata,
+                                        quill::fmt_buffer_t const& formatted_record) noexcept override
+            {
+                // Not using these.
+                (void) thread_id;
+                (void) log_message_timestamp;
+                (void) formatted_record;
+
+                // Log only m_eMinLogLevel or higher to stdout.
+                return metadata.level() >= m_eMinLogLevel;
+            }
+    };
+
 }    // namespace logging
 #endif    // AUTONOMY_LOGGING_H