Merge pull request #359 from MissouriMRDT/topic/#358

Add TagID and Radius to Waypoints

data/Custom_Dictionaries/Autonomy-Dictionary.txt

@@ -6,6 +6,7 @@ accur
 ACCURACYDATA
 ADDMARKERLEG
 ADDOBJECTLEG
+ADDOBSTACLE
 ADDPOSITIONLEG
 algobase
 alloc

src/AutonomyConstants.h

@@ -40,7 +40,7 @@ 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.48";    // The IP address to use for simulation mode.
+    const std::string SIM_IP_ADDRESS = "192.168.69.29";    // The IP address to use for 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.
@@ -247,7 +247,7 @@ namespace constants
     const int TAGDETECT_MAINCAM_MARKER_BORDER_BITS      = 1;                                // This number of bits on the border. A bit is one unit square of the tag.
     const bool TAGDETECT_MAINCAM_USE_ARUCO3_DETECTION   = true;                             // Whether or not to use the newer and faster Aruco detection strategy.
     const int TAGDETECT_MAINCAM_MAX_FPS                 = 30;                               // The max iterations per second of the tag detector.
-    const bool TAGDETECT_MAINCAM_ENABLE_DNN             = true;                             // Whether or not to use DNN detection on top of ArUco.
+    const bool TAGDETECT_MAINCAM_ENABLE_DNN             = false;                            // Whether or not to use DNN detection on top of ArUco.
     const std::string TAGDETECT_MAINCAM_MODEL_PATH = "../data/models/yolo_models/tag/v5n_x320_200epochs/best_edgetpu.tflite";    // The model path to use for detection.
     const float TAGDETECT_MAINCAM_DNN_CONFIDENCE   = 0.4f;    // The minimum confidence to consider a viable AR tag detection.
     const float TAGDETECT_MAINCAM_DNN_NMS_THRESH   = 0.4f;    // The threshold for non-max suppression filtering.
@@ -320,7 +320,6 @@ namespace constants
 
     // Search Pattern State
     const double SEARCH_ANGULAR_STEP_DEGREES = 57.0;    // The amount the angle is incremented in each iteration of the loop (degrees).
-    const double SEARCH_MAX_RADIUS           = 21.0;    //  The maximum radius to cover in the search (meters).
     const double SEARCH_SPIRAL_SPACING       = 1.0;     // The spacing between successive points in the spiral (meters).
     const double SEARCH_ZIGZAG_SPACING       = 1.0;     // The spacing between successive points in the zigzag (meters).
     const double SEARCH_WAYPOINT_PROXIMITY   = 2.0;     // How close a rover must be to a point to have it count as visited.

src/algorithms/SearchPattern.hpp

@@ -61,6 +61,14 @@ namespace searchpattern
         double dStartingY            = stStartingPoint.dNorthing;
         double dCurrentRadius        = 0.0;
 
+        // Check if the MaxRadius is less than 1 meter. If so return an empty vector.
+        if (dMaxRadius < 1)
+        {
+            // Submit logger message.
+            LOG_WARNING(logging::g_qSharedLogger, "MaxRadius is less than 1 meter. Cannot create spiral pattern.");
+            return vWaypoints;
+        }
+
         // Calculate each waypoint. Stop when the radius exceeds the maximum.
         while (dCurrentRadius <= dMaxRadius)
         {
@@ -83,6 +91,16 @@ namespace searchpattern
             dCurrentRadius = geoops::CalculateGeoMeasurement(stStartingPoint, stCurrentCoordinate).dDistanceMeters;
         }
 
+        // Write the search pattern points to the logger, just store the GPS lat/long.
+        std::string szSearchPatternPoints = "Search Pattern Points (Spiral): ";
+        for (geoops::Waypoint& stWaypoint : vWaypoints)
+        {
+            szSearchPatternPoints +=
+                "(" + std::to_string(stWaypoint.GetGPSCoordinate().dLatitude) + ", " + std::to_string(stWaypoint.GetGPSCoordinate().dLongitude) + "), ";
+        }
+        // Submit logger message.
+        LOG_DEBUG(logging::g_qSharedLogger, "{}", szSearchPatternPoints);
+
         return vWaypoints;
     }
 
@@ -119,6 +137,14 @@ namespace searchpattern
         double dStartingY                        = stStartingPointUTM.dNorthing;
         double dCurrentRadius                    = 0.0;
 
+        // Check if the MaxRadius is less than 1 meter. If so return an empty vector.
+        if (dMaxRadius < 1)
+        {
+            // Submit logger message.
+            LOG_WARNING(logging::g_qSharedLogger, "MaxRadius is less than 1 meter. Cannot create spiral pattern.");
+            return vWaypoints;
+        }
+
         // Calculate each waypoint. Stop when the radius exceeds the maximum.
         while (dCurrentRadius <= dMaxRadius)
         {
@@ -141,6 +167,16 @@ namespace searchpattern
             dCurrentRadius = geoops::CalculateGeoMeasurement(stStartingPointUTM, stCurrentCoordinate).dDistanceMeters;
         }
 
+        // Write the search pattern points to the logger, just store the GPS lat/long.
+        std::string szSearchPatternPoints = "Search Pattern Points (Spiral): ";
+        for (geoops::Waypoint& stWaypoint : vWaypoints)
+        {
+            szSearchPatternPoints +=
+                "(" + std::to_string(stWaypoint.GetGPSCoordinate().dLatitude) + ", " + std::to_string(stWaypoint.GetGPSCoordinate().dLongitude) + "), ";
+        }
+        // Submit logger message.
+        LOG_DEBUG(logging::g_qSharedLogger, "{}", szSearchPatternPoints);
+
         return vWaypoints;
     }
 
@@ -176,14 +212,28 @@ namespace searchpattern
         bool bZigNotZag     = true;
         double bCalcSpacing = dSpacing;
 
+        // Check if the width or height is less than 1 meter. If so return an empty vector.
+        if (dWidth < 1 || dHeight < 1 || dSpacing < 1)
+        {
+            // Submit logger message.
+            LOG_WARNING(logging::g_qSharedLogger, "Width or height or spacing is less than 1 meter. Cannot create zigzag pattern.");
+            return vWaypoints;
+        }
+
         // Limit spacing to the width or height.
-        if (bCalcSpacing > dWidth)
+        if (bCalcSpacing > dWidth / 2.0)
         {
-            bCalcSpacing = dWidth;
+            // Submit logger message.
+            LOG_WARNING(logging::g_qSharedLogger, "Spacing is greater than width. Setting spacing to width / 2.");
+            // Set spacing to half the width.
+            bCalcSpacing = dWidth / 2.0 - 1.0;
         }
-        if (bCalcSpacing > dHeight)
+        if (bCalcSpacing > dHeight / 2.0)
         {
-            bCalcSpacing = dHeight;
+            // Submit logger message.
+            LOG_WARNING(logging::g_qSharedLogger, "Spacing is greater than height. Setting spacing to height / 2.");
+            // Set spacing to half the height.
+            bCalcSpacing = dHeight / 2.0 - 1.0;
         }
 
         // Loop until covered entire space of width and height.
@@ -281,6 +331,16 @@ namespace searchpattern
             }
         }
 
+        // Write the search pattern points to the logger, just store the GPS lat/long.
+        std::string szSearchPatternPoints = "Search Pattern Points (Spiral): ";
+        for (geoops::Waypoint& stWaypoint : vWaypoints)
+        {
+            szSearchPatternPoints +=
+                "(" + std::to_string(stWaypoint.GetGPSCoordinate().dLatitude) + ", " + std::to_string(stWaypoint.GetGPSCoordinate().dLongitude) + "), ";
+        }
+        // Submit logger message.
+        LOG_DEBUG(logging::g_qSharedLogger, "{}", szSearchPatternPoints);
+
         // Return the final path.
         return vFilterWaypoints;
     }
@@ -318,14 +378,28 @@ namespace searchpattern
         bool bZigNotZag                        = true;
         double bCalcSpacing                    = dSpacing;
 
+        // Check if the width or height is less than 1 meter. If so return an empty vector.
+        if (dWidth < 1 || dHeight < 1 || dSpacing < 1)
+        {
+            // Submit logger message.
+            LOG_WARNING(logging::g_qSharedLogger, "Width or height or spacing is less than 1 meter. Cannot create zigzag pattern.");
+            return vWaypoints;
+        }
+
         // Limit spacing to the width or height.
-        if (bCalcSpacing > dWidth)
+        if (bCalcSpacing > dWidth / 2.0)
         {
-            bCalcSpacing = dWidth;
+            // Submit logger message.
+            LOG_WARNING(logging::g_qSharedLogger, "Spacing is greater than width. Setting spacing to width / 2.");
+            // Set spacing to half the width.
+            bCalcSpacing = dWidth / 2.0 - 1.0;
         }
-        if (bCalcSpacing > dHeight)
+        if (bCalcSpacing > dHeight / 2.0)
         {
-            bCalcSpacing = dHeight;
+            // Submit logger message.
+            LOG_WARNING(logging::g_qSharedLogger, "Spacing is greater than height. Setting spacing to height / 2.");
+            // Set spacing to half the height.
+            bCalcSpacing = dHeight / 2.0 - 1.0;
         }
 
         // Loop until covered entire space of width and height.
@@ -423,6 +497,16 @@ namespace searchpattern
             }
         }
 
+        // Write the search pattern points to the logger, just store the GPS lat/long.
+        std::string szSearchPatternPoints = "Search Pattern Points (Spiral): ";
+        for (geoops::Waypoint& stWaypoint : vWaypoints)
+        {
+            szSearchPatternPoints +=
+                "(" + std::to_string(stWaypoint.GetGPSCoordinate().dLatitude) + ", " + std::to_string(stWaypoint.GetGPSCoordinate().dLongitude) + "), ";
+        }
+        // Submit logger message.
+        LOG_DEBUG(logging::g_qSharedLogger, "{}", szSearchPatternPoints);
+
         // Return the final path.
         return vFilterWaypoints;
     }

src/handlers/StateMachineHandler.cpp

@@ -339,11 +339,15 @@ void StateMachineHandler::HandleEvent(statemachine::Event eEvent, const bool bSa
     // Acquire write lock for handling events.
     std::unique_lock<std::shared_mutex> lkEventProcessLock(m_muEventMutex);
 
-    // Trigger the event on the current state
-    statemachine::States eNextState = m_pCurrentState->TriggerEvent(eEvent);
+    // Check if the current state is not null and the state machine is running.
+    if (m_pCurrentState != nullptr && this->GetThreadState() == AutonomyThreadState::eRunning)
+    {
+        // Trigger the event on the current state
+        statemachine::States eNextState = m_pCurrentState->TriggerEvent(eEvent);
 
-    // Transition to the next state
-    ChangeState(eNextState, bSaveCurrentState);
+        // Transition to the next state
+        ChangeState(eNextState, bSaveCurrentState);
+    }
 }
 
 /******************************************************************************
@@ -363,6 +367,22 @@ void StateMachineHandler::ClearSavedStates()
     m_pPreviousState = std::make_shared<statemachine::IdleState>();
 }
 
+/******************************************************************************
+ * @brief Clear a saved state based on the given state.
+ *
+ * @param eState - The state to clear from the saved states.
+ *
+ * @author clayjay3 ([email protected])
+ * @date 2025-01-06
+ ******************************************************************************/
+void StateMachineHandler::ClearSavedState(statemachine::States eState)
+{
+    // Acquire write lock for clearing saved states.
+    std::unique_lock<std::shared_mutex> lkStateProcessLock(m_muStateMutex);
+    // Remove all states that match the given state.
+    m_umSavedStates.erase(eState);
+}
+
 /******************************************************************************
  * @brief Accessor for the Current State private member.
  *

src/handlers/StateMachineHandler.h

@@ -174,6 +174,7 @@ class StateMachineHandler : private AutonomyThread<void>
         void HandleEvent(statemachine::Event eEvent, const bool bSaveCurrentState = false);
 
         void ClearSavedStates();
+        void ClearSavedState(statemachine::States eState);
         statemachine::States GetCurrentState() const;
         statemachine::States GetPreviousState() const;
 

src/handlers/WaypointHandler.cpp

@@ -32,6 +32,7 @@ WaypointHandler::WaypointHandler()
     network::g_pRoveCommUDPNode->AddUDPCallback<double>(AddPositionLegCallback, manifest::Autonomy::COMMANDS.find("ADDPOSITIONLEG")->second.DATA_ID);
     network::g_pRoveCommUDPNode->AddUDPCallback<double>(AddMarkerLegCallback, manifest::Autonomy::COMMANDS.find("ADDMARKERLEG")->second.DATA_ID);
     network::g_pRoveCommUDPNode->AddUDPCallback<double>(AddObjectLegCallback, manifest::Autonomy::COMMANDS.find("ADDOBJECTLEG")->second.DATA_ID);
+    network::g_pRoveCommUDPNode->AddUDPCallback<double>(AddObstacleCallback, manifest::Autonomy::COMMANDS.find("ADDOBSTACLE")->second.DATA_ID);
     network::g_pRoveCommUDPNode->AddUDPCallback<uint8_t>(ClearWaypointsCallback, manifest::Autonomy::COMMANDS.find("CLEARWAYPOINTS")->second.DATA_ID);
 }
 

src/handlers/WaypointHandler.h

@@ -146,8 +146,26 @@ class WaypointHandler
             // Not using this.
             (void) stdAddr;
 
+            // Create instance variables.
+            int nMarkerID  = stPacket.vData[2];
+            double dRadius = stPacket.vData[3];
+
+            // Limit the radius to 0-40.
+            if (dRadius < 0)
+            {
+                // Submit logger message.
+                LOG_WARNING(logging::g_qSharedLogger, "Incoming Marker Waypoint Data: Radius is less than 0, setting to 0.");
+                dRadius = 0;
+            }
+            else if (dRadius > 40)
+            {
+                // Submit logger message.
+                LOG_WARNING(logging::g_qSharedLogger, "Incoming Marker Waypoint Data: Radius is greater than 40, setting to 40.");
+                dRadius = 40;
+            }
+
             // Create new waypoint struct with data from the RoveComm packet.
-            geoops::Waypoint stMarkerWaypoint(geoops::GPSCoordinate(stPacket.vData[0], stPacket.vData[1]), geoops::WaypointType::eTagWaypoint);
+            geoops::Waypoint stMarkerWaypoint(geoops::GPSCoordinate(stPacket.vData[0], stPacket.vData[1]), geoops::WaypointType::eTagWaypoint, dRadius, nMarkerID);
 
             // Acquire write lock for writing to waypoints vector.
             std::unique_lock<std::shared_mutex> lkWaypointsLock(m_muWaypointsMutex);
@@ -157,7 +175,12 @@ class WaypointHandler
             lkWaypointsLock.unlock();
 
             // Submit logger message.
-            LOG_INFO(logging::g_qSharedLogger, "Incoming Marker Waypoint Data: Added (lat: {}, lon: {}) to WaypointHandler queue.", stPacket.vData[0], stPacket.vData[1]);
+            LOG_INFO(logging::g_qSharedLogger,
+                     "Incoming Marker Waypoint Data: Added (lat: {}, lon: {}, marker ID: {}, radius: {}) to WaypointHandler queue.",
+                     stPacket.vData[0],
+                     stPacket.vData[1],
+                     nMarkerID,
+                     dRadius);
         };
 
         /******************************************************************************
@@ -173,8 +196,25 @@ class WaypointHandler
             // Not using this.
             (void) stdAddr;
 
+            // Create instance variables.
+            double dRadius = stPacket.vData[2];
+
+            // Limit the radius to 0-40.
+            if (dRadius < 0)
+            {
+                // Submit logger message.
+                LOG_WARNING(logging::g_qSharedLogger, "Incoming Object Waypoint Data: Radius is less than 0, setting to 0.");
+                dRadius = 0;
+            }
+            else if (dRadius > 40)
+            {
+                // Submit logger message.
+                LOG_WARNING(logging::g_qSharedLogger, "Incoming Object Waypoint Data: Radius is greater than 40, setting to 40.");
+                dRadius = 40;
+            }
+
             // Create new waypoint struct with data from the RoveComm packet.
-            geoops::Waypoint stObjectWaypoint(geoops::GPSCoordinate(stPacket.vData[0], stPacket.vData[1]), geoops::WaypointType::eObjectWaypoint);
+            geoops::Waypoint stObjectWaypoint(geoops::GPSCoordinate(stPacket.vData[0], stPacket.vData[1]), geoops::WaypointType::eObjectWaypoint, dRadius);
 
             // Acquire write lock for writing to waypoints vector.
             std::unique_lock<std::shared_mutex> lkWaypointsLock(m_muWaypointsMutex);
@@ -184,7 +224,59 @@ class WaypointHandler
             lkWaypointsLock.unlock();
 
             // Submit logger message.
-            LOG_INFO(logging::g_qSharedLogger, "Incoming Object Waypoint Data: Added (lat: {}, lon: {}) to WaypointHandler queue.", stPacket.vData[0], stPacket.vData[1]);
+            LOG_INFO(logging::g_qSharedLogger,
+                     "Incoming Object Waypoint Data: Added (lat: {}, lon: {}, radius: {}) to WaypointHandler queue.",
+                     stPacket.vData[0],
+                     stPacket.vData[1],
+                     dRadius);
+        };
+
+        /******************************************************************************
+         * @brief Callback function that is called whenever RoveComm receives new ADDOBSTACLE packet.
+         *
+         *
+         * @author clayjay3 ([email protected])
+         * @date 2025-01-06
+         ******************************************************************************/
+        const std::function<void(const rovecomm::RoveCommPacket<double>&, const sockaddr_in&)> AddObstacleCallback =
+            [this](const rovecomm::RoveCommPacket<double>& stPacket, const sockaddr_in& stdAddr)
+        {
+            // Not using this.
+            (void) stdAddr;
+
+            // Create instance variables.
+            double dRadius = stPacket.vData[2];
+
+            // Limit the radius to 0-40.
+            if (dRadius < 0)
+            {
+                // Submit logger message.
+                LOG_WARNING(logging::g_qSharedLogger, "Incoming Obstacle Waypoint Data: Radius is less than 0, setting to 0.");
+                dRadius = 0;
+            }
+            else if (dRadius > 40)
+            {
+                // Submit logger message.
+                LOG_WARNING(logging::g_qSharedLogger, "Incoming Obstacle Waypoint Data: Radius is greater than 40, setting to 40.");
+                dRadius = 40;
+            }
+
+            // Create new waypoint struct with data from the RoveComm packet.
+            geoops::Waypoint stObstacleWaypoint(geoops::GPSCoordinate(stPacket.vData[0], stPacket.vData[1]), geoops::WaypointType::eObstacleWaypoint, dRadius);
+
+            // Acquire write lock for writing to waypoints vector.
+            std::unique_lock<std::shared_mutex> lkWaypointsLock(m_muWaypointsMutex);
+            // Queue waypoint.
+            m_vWaypointList.emplace_back(stObstacleWaypoint);
+            // Unlock mutex.
+            lkWaypointsLock.unlock();
+
+            // Submit logger message.
+            LOG_INFO(logging::g_qSharedLogger,
+                     "Incoming Obstacle Waypoint Data: Added (lat: {}, lon: {}, radius: {}) to WaypointHandler queue.",
+                     stPacket.vData[0],
+                     stPacket.vData[1],
+                     dRadius);
         };
 
         /******************************************************************************