diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 02e3f600..249ea0d4 100755 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -30,8 +30,8 @@ "image": "ghcr.io/missourimrdt/autonomy-jammy:latest", // "image": "ghcr.io/missourimrdt/autonomy-jetpack:latest", // "build": { - // "dockerfile": "Dockerfile_Jammy" - // "dockerfile": "Dockerfile_JetPack" + // "dockerfile": "Dockerfile_Jammy" + // "dockerfile": "Dockerfile_JetPack" // }, // Features to add to the dev container. More info: https://containers.dev/features. // "features": {}, @@ -82,6 +82,8 @@ "C_Cpp.default.compilerPath": "/usr/bin/g++-10", "C_Cpp.default.cppStandard": "c++20", "C_Cpp.default.includePath": [ + "/workspaces/Autonomy_Software/build/_deps/rovecomm_cpp-src/src/", + "/workspaces/Autonomy_Software/build/_deps/rovecomm_cpp-src/src/**", "/usr/local/include/opencv4", "/usr/local/include/opencv4/**", "/usr/local/zed/include", diff --git a/CMakeLists.txt b/CMakeLists.txt index 2508ae6c..161f9120 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -40,7 +40,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 111111d0a59eafcce9f6e3bfb3e6b3f24a60032c # Version 24.0.0 + GIT_TAG 9440581e8b8cc69fab545e3ec6954bc005e6fb32 # Version 24.2.0 GIT_SHALLOW FALSE # Download only without history GIT_PROGRESS TRUE # Forces progress status. ) diff --git a/data/Custom_Dictionaries/Autonomy-Dictionary.txt b/data/Custom_Dictionaries/Autonomy-Dictionary.txt index 6c52ead6..f49312fc 100644 --- a/data/Custom_Dictionaries/Autonomy-Dictionary.txt +++ b/data/Custom_Dictionaries/Autonomy-Dictionary.txt @@ -30,11 +30,13 @@ deque Desaturate devcontainer diffdrive +DISPLAYSTATE dockerfiles dorny doxdocgen Doxygen driveboard +DRIVELEFTRIGHT endcond Extention feedforward @@ -61,6 +63,7 @@ jetpack jspencerpittman jthread keyscan +LEDRGB LEFTCAM MAINCAM MAINCAN @@ -102,7 +105,9 @@ setpoint setpoints SIMCAM softprops +STARTAUTONOMY statechart +STATEDISPLAY statemachine STATEMACHINEHANDLER Struct @@ -111,6 +116,7 @@ Styleguides TAGDETECT TAGDETECTOR Tele +TELEOP tensorflowtag threadpool tparam diff --git a/src/AutonomyConstants.h b/src/AutonomyConstants.h index a84c7bd1..0f0d8af3 100644 --- a/src/AutonomyConstants.h +++ b/src/AutonomyConstants.h @@ -37,6 +37,16 @@ namespace constants const std::string LOGGING_OUTPUT_PATH_ABSOLUTE = "./logs/"; // The absolute to write output logging and video files to. /////////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////////// + //// RoveComm Constants. + /////////////////////////////////////////////////////////////////////////// + + // 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. + /////////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////////// //// Drive Constants. /////////////////////////////////////////////////////////////////////////// diff --git a/src/AutonomyGlobals.cpp b/src/AutonomyGlobals.cpp index be6d557b..a152ca1f 100644 --- a/src/AutonomyGlobals.cpp +++ b/src/AutonomyGlobals.cpp @@ -43,4 +43,8 @@ namespace globals DriveBoard* g_pDriveBoard; MultimediaBoard* g_pMultimediaBoard; NavigationBoard* g_pNavigationBoard; + + // RoveComm Instances: + rovecomm::RoveCommUDP* g_pRoveCommUDPNode; + rovecomm::RoveCommTCP* g_pRoveCommTCPNode; } // namespace globals diff --git a/src/AutonomyGlobals.h b/src/AutonomyGlobals.h index 3a9bb403..53f10931 100644 --- a/src/AutonomyGlobals.h +++ b/src/AutonomyGlobals.h @@ -21,6 +21,7 @@ #include "handlers/WaypointHandler.h" /// \cond +#include #include #include #include @@ -59,6 +60,10 @@ namespace globals extern DriveBoard* g_pDriveBoard; // Global Drive Board Driver extern MultimediaBoard* g_pMultimediaBoard; // Global Multimedia Board Driver extern NavigationBoard* g_pNavigationBoard; // Global Navigation Board Driver + + // RoveComm Instances: + extern rovecomm::RoveCommUDP* g_pRoveCommUDPNode; // Global RoveComm UDP Instance. + extern rovecomm::RoveCommTCP* g_pRoveCommTCPNode; // Global RoveComm TCP Instance. } // namespace globals #endif // AUTONOMY_GLOBALS_H diff --git a/src/drivers/DriveBoard.cpp b/src/drivers/DriveBoard.cpp index 9b9ac7b6..32172c7a 100755 --- a/src/drivers/DriveBoard.cpp +++ b/src/drivers/DriveBoard.cpp @@ -12,8 +12,14 @@ #include "./DriveBoard.h" #include "../AutonomyConstants.h" +#include "../AutonomyGlobals.h" #include "../AutonomyLogging.h" +/// \cond +#include + +/// \endcond + /****************************************************************************** * @brief Construct a new Drive Board::DriveBoard object. * @@ -83,32 +89,37 @@ diffdrive::DrivePowers DriveBoard::CalculateMove(const double dGoalSpeed, /****************************************************************************** * @brief Sets the left and right drive powers of the drive board. * - * @param dLeftSpeed - Left drive speed (-1 to 1) - * @param dRightSpeed - Right drive speed (-1 to 1) + * @param stDrivePowers - A struct containing info about the desired drive powers. * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-09-21 ******************************************************************************/ -void DriveBoard::SendDrive(double dLeftSpeed, double dRightSpeed) +void DriveBoard::SendDrive(diffdrive::DrivePowers& stDrivePowers) { // Limit input values. - dLeftSpeed = std::clamp(dLeftSpeed, -1.0, 1.0); - dRightSpeed = std::clamp(dRightSpeed, -1.0, 1.0); + double dLeftSpeed = std::clamp(stDrivePowers.dLeftDrivePower, -1.0, 1.0); + double dRightSpeed = std::clamp(stDrivePowers.dRightDrivePower, -1.0, 1.0); // Update member variables with new target speeds. m_stDrivePowers.dLeftDrivePower = dLeftSpeed; m_stDrivePowers.dRightDrivePower = dRightSpeed; - // TODO: Uncomment once RoveComm is implemented. This is commented to gid rid of unused variable warnings. - // // Remap -1.0 - 1.0 range to drive power range defined in constants. This is so that the driveboard/rovecomm can understand our input. - // float fDriveBoardLeftPower = numops::MapRange(float(dLeftSpeed), -1.0f, 1.0f, constants::DRIVE_MIN_POWER, constants::DRIVE_MAX_POWER); - // float fDriveBoardRightPower = numops::MapRange(float(dRightSpeed), -1.0f, 1.0f, constants::DRIVE_MIN_POWER, constants::DRIVE_MAX_POWER); - // // Limit the power to max and min effort defined in constants. - // fDriveBoardLeftPower = std::clamp(float(dLeftSpeed), constants::DRIVE_MIN_EFFORT, constants::DRIVE_MAX_EFFORT); - // fDriveBoardRightPower = std::clamp(float(dRightSpeed), constants::DRIVE_MIN_EFFORT, constants::DRIVE_MAX_EFFORT); - + // Remap -1.0 - 1.0 range to drive power range defined in constants. This is so that the driveboard/rovecomm can understand our input. + float fDriveBoardLeftPower = numops::MapRange(float(dLeftSpeed), -1.0f, 1.0f, constants::DRIVE_MIN_POWER, constants::DRIVE_MAX_POWER); + float fDriveBoardRightPower = numops::MapRange(float(dRightSpeed), -1.0f, 1.0f, constants::DRIVE_MIN_POWER, constants::DRIVE_MAX_POWER); + // Limit the power to max and min effort defined in constants. + fDriveBoardLeftPower = std::clamp(float(dLeftSpeed), constants::DRIVE_MIN_EFFORT, constants::DRIVE_MAX_EFFORT); + fDriveBoardRightPower = std::clamp(float(dRightSpeed), constants::DRIVE_MIN_EFFORT, constants::DRIVE_MAX_EFFORT); + + // Construct a RoveComm packet with the drive data. + rovecomm::RoveCommPacket stPacket; + stPacket.unDataId = manifest::Core::COMMANDS.find("DRIVELEFTRIGHT")->second.DATA_ID; + stPacket.unDataCount = manifest::Core::COMMANDS.find("DRIVELEFTRIGHT")->second.DATA_COUNT; + stPacket.eDataType = manifest::Core::COMMANDS.find("DRIVELEFTRIGHT")->second.DATA_TYPE; + stPacket.vData.emplace_back(fDriveBoardLeftPower); + stPacket.vData.emplace_back(fDriveBoardRightPower); // Send drive command over RoveComm to drive board. - // TODO: Add RoveComm sendpacket. + globals::g_pRoveCommUDPNode->SendUDPPacket(stPacket, manifest::Core::IP_ADDRESS.IP_STR.c_str(), constants::ROVECOMM_UDP_PORT); // Submit logger message. LOG_DEBUG(logging::g_qSharedLogger, "Driving at: ({}, {})", m_stDrivePowers.dLeftDrivePower, m_stDrivePowers.dRightDrivePower); @@ -127,8 +138,15 @@ void DriveBoard::SendStop() m_stDrivePowers.dLeftDrivePower = 0.0; m_stDrivePowers.dRightDrivePower = 0.0; + // Construct a RoveComm packet with the drive data. + rovecomm::RoveCommPacket stPacket; + stPacket.unDataId = manifest::Core::COMMANDS.find("DRIVELEFTRIGHT")->second.DATA_ID; + stPacket.unDataCount = manifest::Core::COMMANDS.find("DRIVELEFTRIGHT")->second.DATA_COUNT; + stPacket.eDataType = manifest::Core::COMMANDS.find("DRIVELEFTRIGHT")->second.DATA_TYPE; + stPacket.vData.emplace_back(m_stDrivePowers.dLeftDrivePower); + stPacket.vData.emplace_back(m_stDrivePowers.dRightDrivePower); // Send drive command over RoveComm to drive board. - // TODO: Add RoveComm sendpacket. + globals::g_pRoveCommUDPNode->SendUDPPacket(stPacket, manifest::Core::IP_ADDRESS.IP_STR.c_str(), constants::ROVECOMM_UDP_PORT); } /****************************************************************************** diff --git a/src/drivers/DriveBoard.h b/src/drivers/DriveBoard.h index 6bb568f4..68b1ab68 100755 --- a/src/drivers/DriveBoard.h +++ b/src/drivers/DriveBoard.h @@ -37,6 +37,10 @@ class DriveBoard diffdrive::DrivePowers m_stDrivePowers; // Struct used to store the left and right drive powers of the robot. controllers::PIDController* m_pPID; // The PID controller used for drive towards a heading. + ///////////////////////////////////////// + // Declare private methods. + ///////////////////////////////////////// + public: ///////////////////////////////////////// // Declare public enums that are specific to and used within this class. @@ -52,7 +56,7 @@ class DriveBoard const double dGoalHeading, const double dActualHeading, const diffdrive::DifferentialControlMethod eKinematicsMethod); - void SendDrive(double dLeftSpeed, double dRightSpeed); + void SendDrive(diffdrive::DrivePowers& stDrivePowers); void SendStop(); ///////////////////////////////////////// diff --git a/src/drivers/MultimediaBoard.cpp b/src/drivers/MultimediaBoard.cpp index 91dabba6..a1c6e2d4 100755 --- a/src/drivers/MultimediaBoard.cpp +++ b/src/drivers/MultimediaBoard.cpp @@ -9,72 +9,12 @@ ******************************************************************************/ #include "MultimediaBoard.h" +#include "../AutonomyGlobals.h" -/****************************************************************************** - * @brief This struct serves as a container for RGB values, and provides a - * few overridden constructors for converting from hex to 8-bit RGB values. - * - * - * @author Eli Byrd (edbgkk@mst.edu) - * @date 2023-06-20 - ******************************************************************************/ -struct MultimediaBoard::RGB -{ - public: - // Declare public struct attributes. - double dRed; - double dGreen; - double dBlue; - - /****************************************************************************** - * @brief Construct a new RGB object. - * - * - * @author Eli Byrd (edbgkk@mst.edu) - * @date 2023-06-20 - ******************************************************************************/ - RGB() - { - // Initialize all member variables. - this->dRed = 0; - this->dGreen = 0; - this->dBlue = 0; - } - - /****************************************************************************** - * @brief Construct a new RGB object. - * - * @param iHex - The three hexadecimal digit value containing the RGB values. 0xFFF == 255, 255, 255. - * - * @author Eli Byrd (edbgkk@mst.edu) - * @date 2023-06-20 - ******************************************************************************/ - RGB(int iHex) - { - this->dRed = ((iHex >> 16) & 0xFF); - this->dGreen = ((iHex >> 8) & 0xFF); - this->dBlue = (iHex & 0xFF); - } - - /****************************************************************************** - * @brief Construct a new RGB object. - * - * @param dRed - The red value of the LED panel. (0-255) - * @param dGreen - The green value of the LED panel. (0-255) - * @param dBlue - The blue value of the LED panel. (0-255) - * - * @author Eli Byrd (edbgkk@mst.edu) - * @date 2023-06-20 - ******************************************************************************/ - RGB(double dRed, double dGreen, double dBlue) - { - this->dRed = dRed; - this->dGreen = dGreen; - this->dBlue = dBlue; - } -}; - -///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/// \cond +#include + +/// \endcond /****************************************************************************** * @brief Construct a new Multimedia Board:: Multimedia Board object. @@ -87,9 +27,6 @@ MultimediaBoard::MultimediaBoard() { // Initialize member variables. m_eCurrentLightingState = eOff; - m_dCustomRed = 0.0; - m_dCustomGreen = 0.0; - m_dCustomBlue = 0.0; } /****************************************************************************** @@ -117,27 +54,71 @@ MultimediaBoard::~MultimediaBoard() void MultimediaBoard::SendLightingState(MultimediaBoardLightingState eState) { // Update member variables. - this->m_eCurrentLightingState = eState; + m_eCurrentLightingState = eState; + + // Create new RoveCommPacket. Will be constructed in enum. + rovecomm::RoveCommPacket stPacket; // Decide what lighting operation to execute. switch (eState) { case eOff: + // Construct a RoveComm packet with the lighting data. + stPacket.unDataId = manifest::Core::COMMANDS.find("LEDRGB")->second.DATA_ID; + stPacket.unDataCount = manifest::Core::COMMANDS.find("LEDRGB")->second.DATA_COUNT; + stPacket.eDataType = manifest::Core::COMMANDS.find("LEDRGB")->second.DATA_TYPE; // Use RoveComm to send 0, 0, 0 RGB values. - // TODO: Add RoveComm sendpacket. + stPacket.vData.emplace_back(0); + stPacket.vData.emplace_back(0); + stPacket.vData.emplace_back(0); break; case eCustom: // Use RoveComm to send old custom values previously set. - // TODO: Add RoveComm sendpacket. + this->SendRGB(m_stCustomRGBValues); + break; + + case eTeleOp: + // Construct a RoveComm packet with the lighting data. + stPacket.unDataId = manifest::Core::COMMANDS.find("STATEDISPLAY")->second.DATA_ID; + stPacket.unDataCount = manifest::Core::COMMANDS.find("STATEDISPLAY")->second.DATA_COUNT; + stPacket.eDataType = manifest::Core::COMMANDS.find("STATEDISPLAY")->second.DATA_TYPE; + // Use RoveComm to send BLUE color state value. + stPacket.vData.emplace_back(manifest::Core::DISPLAYSTATE::TELEOP); + break; + + case eAutonomy: + // Construct a RoveComm packet with the lighting data. + stPacket.unDataId = manifest::Core::COMMANDS.find("STATEDISPLAY")->second.DATA_ID; + stPacket.unDataCount = manifest::Core::COMMANDS.find("STATEDISPLAY")->second.DATA_COUNT; + stPacket.eDataType = manifest::Core::COMMANDS.find("STATEDISPLAY")->second.DATA_TYPE; + // Use RoveComm to send RED color state value. + stPacket.vData.emplace_back(manifest::Core::DISPLAYSTATE::AUTONOMY); + break; + + case eReachedGoal: + // Construct a RoveComm packet with the lighting data. + stPacket.unDataId = manifest::Core::COMMANDS.find("STATEDISPLAY")->second.DATA_ID; + stPacket.unDataCount = manifest::Core::COMMANDS.find("STATEDISPLAY")->second.DATA_COUNT; + stPacket.eDataType = manifest::Core::COMMANDS.find("STATEDISPLAY")->second.DATA_TYPE; + // Use RoveComm to send flashing GREEN color state value. + stPacket.vData.emplace_back(manifest::Core::DISPLAYSTATE::REACHED_GOAL); break; default: + // Construct a RoveComm packet with the lighting data. + stPacket.unDataId = manifest::Core::COMMANDS.find("LEDRGB")->second.DATA_ID; + stPacket.unDataCount = manifest::Core::COMMANDS.find("LEDRGB")->second.DATA_COUNT; + stPacket.eDataType = manifest::Core::COMMANDS.find("LEDRGB")->second.DATA_TYPE; // Send lighting state over RoveComm. - // TODO: Add RoveComm sendpacket. + stPacket.vData.emplace_back(0); + stPacket.vData.emplace_back(0); + stPacket.vData.emplace_back(0); break; } + // Send multimedia board lighting state to board over RoveComm. + globals::g_pRoveCommUDPNode->SendUDPPacket(stPacket, manifest::Core::IP_ADDRESS.IP_STR.c_str(), constants::ROVECOMM_UDP_PORT); } /****************************************************************************** @@ -151,16 +132,47 @@ void MultimediaBoard::SendLightingState(MultimediaBoardLightingState eState) void MultimediaBoard::SendRGB(RGB stRGBVal) { // Update custom RGB values. - m_dCustomRed = stRGBVal.dRed; - m_dCustomGreen = stRGBVal.dGreen; - m_dCustomBlue = stRGBVal.dBlue; + m_stCustomRGBValues = stRGBVal; + // Update internal lighting state. + m_eCurrentLightingState = eCustom; + // Construct a RoveComm packet with the lighting data. + rovecomm::RoveCommPacket stPacket; + stPacket.unDataId = manifest::Core::COMMANDS.find("LEDRGB")->second.DATA_ID; + stPacket.unDataCount = manifest::Core::COMMANDS.find("LEDRGB")->second.DATA_COUNT; + stPacket.eDataType = manifest::Core::COMMANDS.find("LEDRGB")->second.DATA_TYPE; + stPacket.vData.emplace_back(stRGBVal.dRed); + stPacket.vData.emplace_back(stRGBVal.dGreen); + stPacket.vData.emplace_back(stRGBVal.dBlue); // Send RGB values to multimedia board over RoveComm. - // TODO: Add RoveComm sendpacket. + globals::g_pRoveCommUDPNode->SendUDPPacket(stPacket, manifest::Core::IP_ADDRESS.IP_STR.c_str(), constants::ROVECOMM_UDP_PORT); } +/****************************************************************************** + * @brief Accessor for the current lighting state of the multimedia board. + * + * @return MultimediaBoard::MultimediaBoardLightingState - An enumerator value representing + * the current lighting state of the board. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2024-03-03 + ******************************************************************************/ MultimediaBoard::MultimediaBoardLightingState MultimediaBoard::GetCurrentLightingState() const { // Return the current lighting state. return m_eCurrentLightingState; } + +/****************************************************************************** + * @brief Accessor for the current custom lighting RGB values. + * + * @return MultimediaBoard::RGB - The custom lighting values stored in an RGB struct. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2024-03-03 + ******************************************************************************/ +MultimediaBoard::RGB MultimediaBoard::GetCustomLightingValues() const +{ + // Return the currently stored custom lighting values. + return m_stCustomRGBValues; +} diff --git a/src/drivers/MultimediaBoard.h b/src/drivers/MultimediaBoard.h index c667cdb1..06d14398 100755 --- a/src/drivers/MultimediaBoard.h +++ b/src/drivers/MultimediaBoard.h @@ -27,7 +27,69 @@ class MultimediaBoard ///////////////////////////////////////// // Structs. - struct RGB; + /****************************************************************************** + * @brief This struct serves as a container for RGB values, and provides a + * few overridden constructors for converting from hex to 8-bit RGB values. + * + * + * @author Eli Byrd (edbgkk@mst.edu) + * @date 2023-06-20 + ******************************************************************************/ + struct RGB + { + public: + // Declare public struct attributes. + double dRed; + double dGreen; + double dBlue; + + /****************************************************************************** + * @brief Construct a new RGB object. + * + * + * @author Eli Byrd (edbgkk@mst.edu) + * @date 2023-06-20 + ******************************************************************************/ + RGB() + { + // Initialize all member variables. + this->dRed = 0; + this->dGreen = 0; + this->dBlue = 0; + } + + /****************************************************************************** + * @brief Construct a new RGB object. + * + * @param iHex - The three hexadecimal digit value containing the RGB values. 0xFFF == 255, 255, 255. + * + * @author Eli Byrd (edbgkk@mst.edu) + * @date 2023-06-20 + ******************************************************************************/ + RGB(int iHex) + { + this->dRed = ((iHex >> 16) & 0xFF); + this->dGreen = ((iHex >> 8) & 0xFF); + this->dBlue = (iHex & 0xFF); + } + + /****************************************************************************** + * @brief Construct a new RGB object. + * + * @param dRed - The red value of the LED panel. (0-255) + * @param dGreen - The green value of the LED panel. (0-255) + * @param dBlue - The blue value of the LED panel. (0-255) + * + * @author Eli Byrd (edbgkk@mst.edu) + * @date 2023-06-20 + ******************************************************************************/ + RGB(double dRed, double dGreen, double dBlue) + { + this->dRed = dRed; + this->dGreen = dGreen; + this->dBlue = dBlue; + } + }; // Enums enum MultimediaBoardLightingState @@ -57,14 +119,13 @@ class MultimediaBoard ///////////////////////////////////////// MultimediaBoardLightingState GetCurrentLightingState() const; + RGB GetCustomLightingValues() const; private: ///////////////////////////////////////// // Declare private member variables. ///////////////////////////////////////// MultimediaBoardLightingState m_eCurrentLightingState; - double m_dCustomRed; - double m_dCustomGreen; - double m_dCustomBlue; + RGB m_stCustomRGBValues; }; #endif diff --git a/src/drivers/NavigationBoard.cpp b/src/drivers/NavigationBoard.cpp index 468a2177..39853510 100755 --- a/src/drivers/NavigationBoard.cpp +++ b/src/drivers/NavigationBoard.cpp @@ -9,86 +9,37 @@ ******************************************************************************/ #include "NavigationBoard.h" +#include "../AutonomyGlobals.h" #include "../AutonomyLogging.h" -/****************************************************************************** - * @brief Construct a new Navigation Board:: Navigation Board object. - * - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-09-23 - ******************************************************************************/ -NavigationBoard::NavigationBoard() {} +/// \cond +// Put implicit #includes here. -/****************************************************************************** - * @brief Destroy the Navigation Board:: Navigation Board object. - * - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-09-23 - ******************************************************************************/ -NavigationBoard::~NavigationBoard() {} +/// \endcond /****************************************************************************** - * @brief Unpack and store data from an IMU packet. - * - * @param stPacket - The special nav board packet containing IMU data. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-09-23 - ******************************************************************************/ -void NavigationBoard::ProcessIMUData(geoops::IMUData stPacket) -{ - // Acquire write lock for writing to IMU struct. - std::unique_lock lkIMUProcessLock(m_muOrientationMutex); - // Update member variables attributes. - m_stOrientation.dPitch = stPacket.dPitch; - m_stOrientation.dRoll = stPacket.dRoll; - m_stOrientation.dHeading = stPacket.dHeading; - - // Submit logger message. - LOG_DEBUG(logging::g_qSharedLogger, "Incoming IMU Data: ({}, {}, {})", m_stOrientation.dPitch, m_stOrientation.dRoll, m_stOrientation.dHeading); -} - -/****************************************************************************** - * @brief Unpack and store data from a GPS packet. + * @brief Construct a new Navigation Board:: Navigation Board object. * - * @param stPacket - The special nav board struct containing GPS data. * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-09-23 ******************************************************************************/ -void NavigationBoard::ProcessGPSData(geoops::GPSCoordinate stPacket) +NavigationBoard::NavigationBoard() { - // Acquire write lock for writing to GPS struct. - std::unique_lock lkGPSProcessLock(m_muLocationMutex); - // Submit logger message. - LOG_DEBUG(logging::g_qSharedLogger, - "Incoming GPS Data: ({} lat, {} lon, {} alt, {} acc)", - stPacket.dLatitude, - stPacket.dLongitude, - stPacket.dAltitude, - stPacket.d2DAccuracy); - - // Store GPS data in member variable. - m_stLocation = stPacket; + // Set RoveComm callbacks. + globals::g_pRoveCommUDPNode->AddUDPCallback(ProcessGPSData, constants::ROVECOMM_UDP_PORT); + globals::g_pRoveCommUDPNode->AddUDPCallback(ProcessAccuracyData, constants::ROVECOMM_UDP_PORT); + globals::g_pRoveCommUDPNode->AddUDPCallback(ProcessCompassData, constants::ROVECOMM_UDP_PORT); } /****************************************************************************** - * @brief Accessor for most recent IMU data received from NavBoard. + * @brief Destroy the Navigation Board:: Navigation Board object. * - * @return geoops::IMUData - Struct containing roll, pitch, and yaw/heading info. * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-09-23 ******************************************************************************/ -geoops::IMUData NavigationBoard::GetIMUData() -{ - // Acquire read lock for getting IMU struct. - std::shared_lock lkIMUProcessLock(m_muOrientationMutex); - // Return the orientation struct member variable. - return m_stOrientation; -} +NavigationBoard::~NavigationBoard() {} /****************************************************************************** * @brief Accessor for most recent GPS data received from NavBoard. @@ -101,7 +52,7 @@ geoops::IMUData NavigationBoard::GetIMUData() geoops::GPSCoordinate NavigationBoard::GetGPSData() { // Acquire read lock for getting UTM struct. - std::shared_lock lkIMUProcessLock(m_muLocationMutex); + std::shared_lock lkGPSProcessLock(m_muLocationMutex); // Convert the currently stored UTM coord to GPS and return. return m_stLocation; } @@ -118,6 +69,21 @@ geoops::GPSCoordinate NavigationBoard::GetGPSData() geoops::UTMCoordinate NavigationBoard::GetUTMData() { // Acquire read lock for getting UTM struct. - std::shared_lock lkIMUProcessLock(m_muLocationMutex); + std::shared_lock lkGPSProcessLock(m_muLocationMutex); return geoops::ConvertGPSToUTM(m_stLocation); } + +/****************************************************************************** + * @brief Accessor for the most recent compass heading received from the NavBoard. + * + * @return double - The last known compass heading. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2024-03-03 + ******************************************************************************/ +double NavigationBoard::GetHeading() +{ + // Acquire read lock for getting compass double. + std::shared_lock lkCompassProcessLock(m_muHeadingMutex); + return m_dHeading; +} diff --git a/src/drivers/NavigationBoard.h b/src/drivers/NavigationBoard.h index a86cbf4c..2656fc1f 100755 --- a/src/drivers/NavigationBoard.h +++ b/src/drivers/NavigationBoard.h @@ -14,6 +14,8 @@ #include "../util/GeospatialOperations.hpp" /// \cond +#include +#include #include /// \endcond @@ -48,31 +50,102 @@ class NavigationBoard // Getters ///////////////////////////////////////// - geoops::IMUData GetIMUData(); geoops::GPSCoordinate GetGPSData(); geoops::UTMCoordinate GetUTMData(); + double GetHeading(); private: ///////////////////////////////////////// - // Declare private methods. + // Declare private member variables. ///////////////////////////////////////// - void ProcessIMUData(geoops::IMUData stPacket); - void ProcessGPSData(geoops::GPSCoordinate stPacket); + geoops::GPSCoordinate m_stLocation; // Store current global position in UTM format. + double m_dHeading; // Store current GPS heading. + std::shared_mutex m_muLocationMutex; // Mutex for acquiring read and write lock on location member variable. + std::shared_mutex m_muHeadingMutex; // Mutex for acquiring read and write lock on heading member variable. ///////////////////////////////////////// - // Declare private member variables. + // Declare private methods. ///////////////////////////////////////// - geoops::GPSCoordinate m_stLocation; // Store current global position in UTM format. - geoops::IMUData m_stOrientation; // Store current IMU orientation. - std::shared_mutex m_muLocationMutex; // Mutex for acquiring read and write lock on location member variable. - std::shared_mutex m_muOrientationMutex; // Mutex for acquiring read and write lock on orientation member variable. - - // TODO: RoveComm Node - - // TODO: RoveComm Callback -> IMU Data - // TODO: RoveComm Callback -> GPS Data + /****************************************************************************** + * @brief Callback function that is called whenever RoveComm receives new GPS data. + * + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2024-03-03 + ******************************************************************************/ + const std::function&, const sockaddr_in&)> ProcessGPSData = + [this](const rovecomm::RoveCommPacket& stPacket, const sockaddr_in& stdAddr) + { + // Not using this. + (void) stdAddr; + + // Acquire write lock for writing to GPS struct. + std::unique_lock lkGPSProcessLock(m_muLocationMutex); + // Repack data from RoveCommPacket into member variable. + m_stLocation.dLatitude = stPacket.vData[0]; + m_stLocation.dLongitude = stPacket.vData[1]; + m_stLocation.dAltitude = stPacket.vData[2]; + // Unlock mutex. + lkGPSProcessLock.unlock(); + + // Submit logger message. + LOG_DEBUG(logging::g_qSharedLogger, "Incoming GPS Data: ({} lat, {} lon, {} alt)", m_stLocation.dLatitude, m_stLocation.dLongitude, m_stLocation.dAltitude); + }; + + /****************************************************************************** + * @brief Callback function that is called whenever RoveComm receives new Accuracy data. + * + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2024-03-03 + ******************************************************************************/ + const std::function&, const sockaddr_in&)> ProcessAccuracyData = + [this](const rovecomm::RoveCommPacket& stPacket, const sockaddr_in& stdAddr) + { + // Not using this. + (void) stdAddr; + + // Acquire write lock for writing to GPS struct. + std::unique_lock lkGPSProcessLock(m_muLocationMutex); + // Repack data from RoveCommPacket into member variable. + m_stLocation.d2DAccuracy = stPacket.vData[0]; + m_stLocation.d3DAccuracy = stPacket.vData[1]; + // Unlock mutex. + lkGPSProcessLock.unlock(); + + // Submit logger message. + LOG_DEBUG(logging::g_qSharedLogger, + "Incoming Accuracy Data: (2D: {}, 3D: {}, Compass: {})", + m_stLocation.d2DAccuracy, + m_stLocation.d3DAccuracy, + stPacket.vData[2]); + }; + + /****************************************************************************** + * @brief Callback function that is called whenever RoveComm receives new Compass data. + * + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2024-03-03 + ******************************************************************************/ + const std::function&, const sockaddr_in&)> ProcessCompassData = + [this](const rovecomm::RoveCommPacket& stPacket, const sockaddr_in& stdAddr) + { + // Not using this. + (void) stdAddr; + + // Acquire write lock for writing to GPS struct. + std::unique_lock lkCompassProcessLock(m_muHeadingMutex); + // Repack data from RoveCommPacket into member variable. + m_dHeading = stPacket.vData[0]; + // Unlock mutex. + lkCompassProcessLock.unlock(); + + // Submit logger message. + LOG_DEBUG(logging::g_qSharedLogger, "Incoming Compass Data: {}", m_dHeading); + }; }; #endif // NAVIGATIONBOARD_H diff --git a/src/main.cpp b/src/main.cpp index 1c2893aa..03213d4e 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -88,6 +88,33 @@ int main() sigemptyset(&stSigBreak.sa_mask); sigaction(SIGINT, &stSigBreak, nullptr); + ///////////////////////////////////////// + // Setup global objects. + ///////////////////////////////////////// + // Initialize RoveComm. + globals::g_pRoveCommUDPNode = new rovecomm::RoveCommUDP(); + globals::g_pRoveCommTCPNode = new rovecomm::RoveCommTCP(); + // Start RoveComm instances bound on ports. + bool bRoveCommUDPInitSuccess = globals::g_pRoveCommUDPNode->InitUDPSocket(constants::ROVECOMM_UDP_PORT); + bool bRoveCommTCPInitSuccess = globals::g_pRoveCommTCPNode->InitTCPSocket(constants::ROVECOMM_TCP_INTERFACE_IP.c_str(), constants::ROVECOMM_TCP_PORT); + // Check if RoveComm was successfully initialized. + if (!bRoveCommUDPInitSuccess || !bRoveCommTCPInitSuccess) + { + // Submit logger message. + LOG_CRITICAL(logging::g_qSharedLogger, + "RoveComm did not initialize properly! UDPNode Status: {}, TCPNode State: {}", + bRoveCommUDPInitSuccess, + bRoveCommTCPInitSuccess); + + // Since RoveComm is crucial, stop code. + bMainStop = true; + } + else + { + // Submit logger message. + LOG_INFO(logging::g_qSharedLogger, "RoveComm UDP and TCP nodes successfully initialized."); + } + // Initialize drivers. globals::g_pDriveBoard = new DriveBoard(); globals::g_pMultimediaBoard = new MultimediaBoard(); @@ -109,8 +136,6 @@ int main() // TEST: Send Start Command to enter navigating state. globals::g_pStateMachineHandler->HandleEvent(statemachine::Event::eStart); - // TODO: Initialize RoveComm. - ///////////////////////////////////////// // Declare local variables used in main loop. ///////////////////////////////////////// @@ -184,18 +209,24 @@ int main() globals::g_pStateMachineHandler->StopStateMachine(); globals::g_pTagDetectionHandler->StopAllDetectors(); globals::g_pCameraHandler->StopAllCameras(); + globals::g_pRoveCommUDPNode->CloseUDPSocket(); + globals::g_pRoveCommTCPNode->CloseTCPSocket(); // Delete dynamically allocated objects. delete globals::g_pStateMachineHandler; delete globals::g_pTagDetectionHandler; delete globals::g_pCameraHandler; delete globals::g_pWaypointHandler; + delete globals::g_pRoveCommUDPNode; + delete globals::g_pRoveCommTCPNode; // Set dangling pointers to null. globals::g_pStateMachineHandler = nullptr; globals::g_pTagDetectionHandler = nullptr; globals::g_pCameraHandler = nullptr; globals::g_pWaypointHandler = nullptr; + globals::g_pRoveCommUDPNode = nullptr; + globals::g_pRoveCommTCPNode = nullptr; } // Submit logger message that program is done cleaning up and is now exiting.