Fixed the Buffer Stream invalid length bug

source/LibMultiSense/details/dispatch.cc

@@ -91,12 +91,6 @@
 #include "MultiSense/details/wire/SecondaryAppGetRegisteredAppsMessage.hh"
 #include "MultiSense/details/wire/SecondaryAppRegisteredAppsMessage.hh"
 
-#include "MultiSense/details/wire/FeatureDetectorConfigMessage.hh"
-#include "MultiSense/details/wire/FeatureDetectorGetConfigMessage.hh"
-#include "MultiSense/details/wire/FeatureDetectorControlMessage.hh"
-#include "MultiSense/details/wire/FeatureDetectorMessage.hh"
-#include "MultiSense/details/wire/FeatureDetectorMetaMessage.hh"
-
 #include <limits>
 
 #define __STDC_FORMAT_MACROS
@@ -250,24 +244,6 @@ void impl::dispatchAprilTagDetections(apriltag::Header& header)
     m_channelStatistics.numDispatchedGroundSurfaceSpline += 1;
 }
 
-//
-// Publish a feature detection event
-
-// void impl::dispatchFeatureDetections(feature_detector::Header& header)
-// {
-//     utility::ScopedLock lock(m_dispatchLock);
-//
-//     std::list<FeatureDetectorListener*>::const_iterator it;
-//
-//     for(it  = m_featureDetectorListeners.begin();
-//         it != m_featureDetectorListeners.end();
-//         ++ it)
-//         (*it)->dispatch(header);
-//
-//     utility::ScopedLock statsLock(m_statisticsLock);
-//     m_channelStatistics.numDispatchedFeatureDetections++;
-// }
-
 //
 // Publish Secondary App Data
 
@@ -636,7 +612,7 @@ void impl::dispatch(utility::BufferStreamWriter& buffer)
         header.source                     = SecondaryApp.source | ((uint64_t)SecondaryApp.sourceExtended << 32);
         header.timeSeconds                = SecondaryApp.timeSeconds;
         header.timeMicroSeconds           = SecondaryApp.timeMicroSeconds;
-        header.length                     = SecondaryApp.length;
+        header.secondaryAppDataLength     = SecondaryApp.length;
         header.secondaryAppDataP          = SecondaryApp.dataP;
         header.secondaryAppMetadataP      = metaP->dataP;
         header.secondaryAppMetadataLength = metaP->dataLength;
@@ -733,9 +709,6 @@ void impl::dispatch(utility::BufferStreamWriter& buffer)
     case MSG_ID(wire::PtpStatusResponse::ID):
         m_messages.store(wire::PtpStatusResponse(stream, version));
         break;
-    // case MSG_ID(wire::FeatureDetectorConfig::ID):
-    //     m_messages.store(wire::FeatureDetectorConfig(stream, version));
-    //     break;
     default:
 
         CRL_DEBUG("unknown message received: id=%d, version=%d\n",

source/LibMultiSense/details/public.cc

@@ -290,27 +290,6 @@ Status impl::addIsolatedCallback(apriltag::Callback callback,
     return Status_Ok;
 }
 
-//
-// Adds a new feature detector listener
-//
-// Status impl::addIsolatedCallback(feature_detector::Callback callback,
-//                                  void *userDataP)
-// {
-//     try {
-//
-//         utility::ScopedLock lock(m_dispatchLock);
-//         m_featureDetectorListeners.push_back(new FeatureDetectorListener(callback,
-//                                                0,
-//                                                userDataP,
-//                                                MAX_USER_FEATURE_DETECTOR_QUEUE_SIZE));
-//
-//     } catch (const std::exception& e) {
-//         CRL_DEBUG("exception: %s\n", e.what());
-//         return Status_Exception;
-//     }
-//     return Status_Ok;
-// }
-
 //
 // Removes an image listener
 
@@ -561,7 +540,6 @@ Status impl::secondaryAppDataExtract(feature_detector::Header &header, const sec
   wire::FeatureDetector featureDetector(stream, 1); //TODO Version check
 
   utility::BufferStreamReader metaStream(reinterpret_cast<const uint8_t *>(orig.secondaryAppMetadataP), orig.secondaryAppMetadataLength);
-
   wire::FeatureDetectorMeta _meta(metaStream, 1);
 
   header.source         = featureDetector.source;

source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh

@@ -3088,8 +3088,6 @@ public:
 
     /** DataSource corresponding to secondaryAppDataP*/
     DataSource  source;
-    /** length of the secondaryAppData */
-    uint32_t    length;
     /** Unique ID used to describe an secondaryAppData. FrameIds increase sequentally from the device */
     int64_t     frameId;
     /** The time seconds value corresponding to when  the secondaryAppData was captured*/

source/LibMultiSense/include/MultiSense/details/channel.hh

@@ -315,13 +315,12 @@ private:
     static CRL_CONSTEXPR uint32_t DEFAULT_ACK_ATTEMPTS              = 5;
     static CRL_CONSTEXPR uint32_t IMAGE_META_CACHE_DEPTH            = 4;
     static CRL_CONSTEXPR uint32_t SECONDARY_APP_META_CACHE_DEPTH    = 4;
-    static CRL_CONSTEXPR uint32_t FEATURE_DETECTOR_META_CACHE_DEPTH = 4;
     static CRL_CONSTEXPR uint32_t UDP_TRACKER_CACHE_DEPTH           = 4;
     static CRL_CONSTEXPR uint32_t TIME_SYNC_OFFSET_DECAY            = 8;
 
 #if __cplusplus > 199711L
     static_assert(RX_POOL_LARGE_BUFFER_COUNT > IMAGE_META_CACHE_DEPTH,            "Image metadata depth cache too large");
-    static_assert(RX_POOL_LARGE_BUFFER_COUNT > FEATURE_DETECTOR_META_CACHE_DEPTH, "Feature detector metadata depth cache too large");
+    static_assert(RX_POOL_LARGE_BUFFER_COUNT > SECONDARY_APP_META_CACHE_DEPTH,    "Secondary Application metadata depth cache too large");
     static_assert(RX_POOL_LARGE_BUFFER_COUNT > UDP_TRACKER_CACHE_DEPTH,           "UDP depth cache too large");
     static_assert(RX_POOL_SMALL_BUFFER_COUNT > UDP_TRACKER_CACHE_DEPTH,           "UDP depth cache too large");
 #endif
@@ -336,11 +335,17 @@ private:
     static CRL_CONSTEXPR uint32_t MAX_USER_IMAGE_QUEUE_SIZE = 8;
     static CRL_CONSTEXPR uint32_t MAX_USER_LASER_QUEUE_SIZE = 8;
     static CRL_CONSTEXPR uint32_t MAX_USER_COMPRESSED_IMAGE_QUEUE_SIZE = 8;
+    static CRL_CONSTEXPR uint32_t MAX_USER_SECONDARY_APP_QUEUE_SIZE    = 8;
+    static CRL_CONSTEXPR uint32_t MAX_USER_GROUND_SURFACE_QUEUE_SIZE   = 8;
+    static CRL_CONSTEXPR uint32_t MAX_USER_APRILTAG_QUEUE_SIZE         = 8;
 
 #if __cplusplus > 199711L
     static_assert(RX_POOL_LARGE_BUFFER_COUNT > MAX_USER_IMAGE_QUEUE_SIZE, "Image queue too large");
     static_assert(RX_POOL_LARGE_BUFFER_COUNT > MAX_USER_LASER_QUEUE_SIZE, "Laser queue too large");
     static_assert(RX_POOL_LARGE_BUFFER_COUNT > MAX_USER_COMPRESSED_IMAGE_QUEUE_SIZE, "Compressed image queue too large");
+    static_assert(RX_POOL_LARGE_BUFFER_COUNT > MAX_USER_SECONDARY_APP_QUEUE_SIZE, "Secondary App queue too large");
+    static_assert(RX_POOL_LARGE_BUFFER_COUNT > MAX_USER_GROUND_SURFACE_QUEUE_SIZE,"Ground Surface queue too large");
+    static_assert(RX_POOL_LARGE_BUFFER_COUNT > MAX_USER_APRILTAG_QUEUE_SIZE,      "April Tag queue too large");
 #endif
 
     //
@@ -349,10 +354,6 @@ private:
 
     static CRL_CONSTEXPR uint32_t MAX_USER_PPS_QUEUE_SIZE = 2;
     static CRL_CONSTEXPR uint32_t MAX_USER_IMU_QUEUE_SIZE = 64;
-    static CRL_CONSTEXPR uint32_t MAX_USER_GROUND_SURFACE_QUEUE_SIZE   = 8;
-    static CRL_CONSTEXPR uint32_t MAX_USER_APRILTAG_QUEUE_SIZE         = 8;
-    static CRL_CONSTEXPR uint32_t MAX_USER_FEATURE_DETECTOR_QUEUE_SIZE = 8;
-    static CRL_CONSTEXPR uint32_t MAX_USER_SECONDARY_APP_QUEUE_SIZE    = 8;
 
 
     //

source/LibMultiSense/include/MultiSense/details/utility/BufferStream.hh

@@ -93,7 +93,7 @@ public:
     void seek(std::size_t idx)  {
 
         if (idx > m_size)
-            CRL_EXCEPTION("invalid seek location %d, [0, %d] valid\n",
+            CRL_EXCEPTION("invalid seek location %lu, [0, %lu] valid\n",
                           idx, m_size);
         m_tell = idx;
     };

source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc

@@ -348,15 +348,15 @@ void imageCallback(const image::Header& header,
 void featureDetectorCallback(const secondary_app::Header& header,
                    void                *userDataP)
 {
-
     feature_detector::Header fHeader;
     UserData *userData = reinterpret_cast<UserData*>(userDataP);
-    Status s = userData->channelP->secondaryAppDataExtract(fHeader, header);
     void * buffer = userData->channelP->reserveCallbackBuffer();
+    Status s = userData->channelP->secondaryAppDataExtract(fHeader, header);
 
     if (s != Status_Ok)
     {
-      fprintf(stderr, "%s Error failed extraction\n", __func__ );
+      fprintf(stderr, "%s Error Secondary App Data extraction failed\n", __func__ );
+      userData->channelP->releaseCallbackBuffer(buffer);
       return;
     }
 
@@ -374,9 +374,9 @@ void featureDetectorCallback(const secondary_app::Header& header,
         {
             it->second.featureTime = std::chrono::system_clock::now();
             std::cout << "Feature received after image " << fHeader.frameId
-            << " Delta: "
-            << std::chrono::duration_cast<std::chrono::milliseconds>(it->second.featureTime - it->second.imageTime).count()
-            << "ms\n";
+              << " Delta: "
+              << std::chrono::duration_cast<std::chrono::milliseconds>(it->second.featureTime - it->second.imageTime).count()
+              << "ms\n";
             userData->elapsedTime.erase(it);
         }
     }