Purged some feature detector data, added frameId to metadata

source/LibMultiSense/details/channel.cc

@@ -79,7 +79,8 @@ impl::impl(const std::string& address, const RemoteHeadChannel& cameraId, const
     m_rxLargeBufferPool(),
     m_rxSmallBufferPool(),
     m_imageMetaCache(IMAGE_META_CACHE_DEPTH),
-    m_featureDetectorMetaCache(FEATURE_DETECTOR_META_CACHE_DEPTH),
+    // m_featureDetectorMetaCache(FEATURE_DETECTOR_META_CACHE_DEPTH),
+    m_secondaryAppMetaCache(SECONDARY_APP_META_CACHE_DEPTH),
     m_udpAssemblerMap(),
     m_dispatchLock(),
     m_streamLock(),
@@ -93,7 +94,7 @@ impl::impl(const std::string& address, const RemoteHeadChannel& cameraId, const
     m_imuListeners(),
     m_compressedImageListeners(),
     m_secondaryAppListeners(),
-    m_featureDetectorListeners(),
+    // m_featureDetectorListeners(),
     m_watch(),
     m_messages(),
     m_streamsEnabled(0),
@@ -270,11 +271,11 @@ void impl::cleanup()
         its != m_secondaryAppListeners.end();
         its ++)
         delete *its;
-    std::list<FeatureDetectorListener*>::const_iterator itf;
-    for(itf  = m_featureDetectorListeners.begin();
-        itf != m_featureDetectorListeners.end();
-        ++ itf)
-        delete *itf;
+    // std::list<FeatureDetectorListener*>::const_iterator itf;
+    // for(itf  = m_featureDetectorListeners.begin();
+    //     itf != m_featureDetectorListeners.end();
+    //     ++ itf)
+    //     delete *itf;
     BufferPool::const_iterator it;
     for(it  = m_rxLargeBufferPool.begin();
         it != m_rxLargeBufferPool.end();
@@ -291,7 +292,7 @@ void impl::cleanup()
     m_imuListeners.clear();
     m_secondaryAppListeners.clear();
     m_compressedImageListeners.clear();
-    m_featureDetectorListeners.clear();
+    // m_featureDetectorListeners.clear();
     m_rxLargeBufferPool.clear();
     m_rxSmallBufferPool.clear();
 

source/LibMultiSense/details/dispatch.cc

@@ -253,20 +253,20 @@ void impl::dispatchAprilTagDetections(apriltag::Header& header)
 //
 // 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++;
-}
+// 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
@@ -282,6 +282,9 @@ void impl::dispatchSecondaryApplication(utility::BufferStream& buffer,
         it != m_secondaryAppListeners.end();
         it ++)
         (*it)->dispatch(buffer, header);
+
+    utility::ScopedLock statsLock(m_statisticsLock);
+    m_channelStatistics.numDispatchedSecondary++;
 }
 
 
@@ -621,6 +624,7 @@ void impl::dispatch(utility::BufferStreamWriter& buffer)
     }
     case MSG_ID(wire::SecondaryAppData::ID):
     {
+        printf("[%s] secondaryAppData\n", __func__ );
         wire::SecondaryAppData SecondaryApp(stream, version);
 
         secondary_app::Header header;
@@ -674,14 +678,15 @@ void impl::dispatch(utility::BufferStreamWriter& buffer)
     //     dispatchFeatureDetections(header);
     //     break;
     // }
-    case MSG_ID(wire::FeatureDetectorMeta::ID):
+    case MSG_ID(wire::SecondaryAppMetadata::ID):
     {
-        wire::FeatureDetectorMeta *metaP = new (std::nothrow) wire::FeatureDetectorMeta(stream, version);
+        printf("[%s] secondaryAppMetaData\n", __func__ );
+        wire::SecondaryAppMetadata *metaP = new (std::nothrow) wire::SecondaryAppMetadata(stream, version);
 
         if (NULL == metaP)
             CRL_EXCEPTION_RAW("unable to allocate metadata");
 
-        m_featureDetectorMetaCache.insert(metaP->frameId, metaP); // destroys oldest
+        m_secondaryAppMetaCache.insert(metaP->frameId, metaP); // destroys oldest
         break;
     }
     case MSG_ID(wire::Ack::ID):

source/LibMultiSense/details/public.cc

@@ -290,24 +290,24 @@ Status impl::addIsolatedCallback(apriltag::Callback callback,
 
 //
 // 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;
-}
+//
+// 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
@@ -555,31 +555,31 @@ Status impl::removeIsolatedCallback(secondary_app::Callback callback)
 
 //
 // Removes a feature detector listener
-
-Status impl::removeIsolatedCallback(feature_detector::Callback callback)
-{
-    try {
-        utility::ScopedLock lock(m_dispatchLock);
-
-        std::list<FeatureDetectorListener*>::iterator it;
-        for(it  = m_featureDetectorListeners.begin();
-            it != m_featureDetectorListeners.end();
-            ++ it) {
-
-            if ((*it)->callback() == callback) {
-                delete *it;
-                m_featureDetectorListeners.erase(it);
-                return Status_Ok;
-            }
-        }
-
-    } catch (const std::exception& e) {
-        CRL_DEBUG("exception: %s\n", e.what());
-        return Status_Exception;
-    }
-
-    return Status_Error;
-}
+// 
+// Status impl::removeIsolatedCallback(feature_detector::Callback callback)
+// {
+//     try {
+//         utility::ScopedLock lock(m_dispatchLock);
+//
+//         std::list<FeatureDetectorListener*>::iterator it;
+//         for(it  = m_featureDetectorListeners.begin();
+//             it != m_featureDetectorListeners.end();
+//             ++ it) {
+//
+//             if ((*it)->callback() == callback) {
+//                 delete *it;
+//                 m_featureDetectorListeners.erase(it);
+//                 return Status_Ok;
+//             }
+//         }
+//
+//     } catch (const std::exception& e) {
+//         CRL_DEBUG("exception: %s\n", e.what());
+//         return Status_Exception;
+//     }
+//
+//     return Status_Error;
+// }
 
 //
 // Reserve the current callback buffer being used in a dispatch thread

source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh

@@ -390,8 +390,8 @@ public:
      * @return A crl::multisense::Status indicating if the callback registration
      * succeeded or failed
      */
-    virtual Status addIsolatedCallback(feature_detector::Callback callback,
-                                       void         *userDataP=NULL) = 0;
+    // virtual Status addIsolatedCallback(feature_detector::Callback callback,
+                                       // void         *userDataP=NULL) = 0;
 
     /**
      * Unregister a user defined image::Callback. This stops the callback
@@ -487,7 +487,7 @@ public:
      * succeeded or failed
      */
 
-    virtual Status removeIsolatedCallback(feature_detector::Callback callback) = 0;
+    // virtual Status removeIsolatedCallback(feature_detector::Callback callback) = 0;
 
     /**
      * Unregister a user defined secondary_app::Callback. This stops the callback

source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh

@@ -4207,7 +4207,8 @@ struct ChannelStatistics
         numDispatchedImu(0),
         numDispatchedCompressedImage(0),
         numDispatchedGroundSurfaceSpline(0),
-        numDispatchedAprilTagDetections(0)
+        numDispatchedAprilTagDetections(0),
+        numDispatchedSecondary(0)
     {
     };
 
@@ -4258,8 +4259,8 @@ struct ChannelStatistics
     std::size_t numDispatchedAprilTagDetections;
 
     //
-    // The number of dispatached feature detections
-    std::size_t numDispatchedFeatureDetections;
+    // The number of dispatached secondary application
+    std::size_t numDispatchedSecondary;
 };
 
 class MULTISENSE_API SecondaryAppConfig {

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

@@ -48,7 +48,7 @@
 #include "MultiSense/details/storage.hh"
 #include "MultiSense/details/wire/Protocol.hh"
 #include "MultiSense/details/wire/ImageMetaMessage.hh"
-#include "MultiSense/details/wire/FeatureDetectorMetaMessage.hh"
+#include "MultiSense/details/wire/SecondaryAppMetaMessage.hh"
 #include "MultiSense/details/wire/StatusResponseMessage.hh"
 #include "MultiSense/details/wire/PtpStatusResponseMessage.hh"
 #include "MultiSense/details/wire/VersionResponseMessage.hh"
@@ -126,8 +126,8 @@ public:
     virtual Status addIsolatedCallback   (secondary_app::Callback callback,
                                           void         *userDataP);
 
-    virtual Status addIsolatedCallback   (feature_detector::Callback callback,
-                                          void         *userDataP);
+    // virtual Status addIsolatedCallback   (feature_detector::Callback callback,
+                                          // void         *userDataP);
 
     virtual Status removeIsolatedCallback(image::Callback callback);
     virtual Status removeIsolatedCallback(lidar::Callback callback);
@@ -137,7 +137,7 @@ public:
     virtual Status removeIsolatedCallback(ground_surface::Callback   callback);
     virtual Status removeIsolatedCallback(apriltag::Callback   callback);
     virtual Status removeIsolatedCallback(secondary_app::Callback   callback);
-    virtual Status removeIsolatedCallback(feature_detector::Callback   callback);
+    // virtual Status removeIsolatedCallback(feature_detector::Callback   callback);
 
     virtual void*  reserveCallbackBuffer ();
     virtual Status releaseCallbackBuffer (void *referenceP);
@@ -311,6 +311,7 @@ private:
     static double DEFAULT_ACK_TIMEOUT ()         { return 0.5; }
     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;
@@ -457,7 +458,12 @@ private:
     //
     // A cache of feature detector meta data
 
-    DepthCache<int64_t, wire::FeatureDetectorMeta> m_featureDetectorMetaCache;
+    // DepthCache<int64_t, wire::FeatureDetectorMeta> m_featureDetectorMetaCache;
+
+    //
+    // A cache of secondary application meta data
+
+    DepthCache<int64_t, wire::SecondaryAppMetadata> m_secondaryAppMetaCache;
 
     //
     // A map of custom UDP assemblers
@@ -503,7 +509,7 @@ private:
     std::list<GroundSurfaceSplineListener*>     m_groundSurfaceSplineListeners;
     std::list<AprilTagDetectionListener*>       m_aprilTagDetectionListeners;
     std::list<SecondaryAppListener*>            m_secondaryAppListeners;
-    std::list<FeatureDetectorListener*>         m_featureDetectorListeners;
+    // std::list<FeatureDetectorListener*>      m_featureDetectorListeners;
 
     //
     // A message signal interface

source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh

@@ -58,14 +58,16 @@ static CRL_CONSTEXPR VersionType VERSION = 1;
     VersionType version;
 #endif // SENSORPOD_FIRMWARE
 
-    size_t dataLength;
-    void * dataP;
+    int64_t frameId;
+    size_t  dataLength;
+    void *  dataP;
 
     SecondaryAppMetaHeader():
 #ifdef SENSORDPOD_FIRMWARE
         id(ID),
         version(VERSION),
 #endif // SENSORPOD_FIRMWARE
+        frameId(0),
         dataLength(0),
         dataP(nullptr)
       {};
@@ -81,7 +83,7 @@ public:
     // Constructors
 
     SecondaryAppMetadata(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);};
-    SecondaryAppMetadata() : dataLength(0), dataP(NULL) {};
+    SecondaryAppMetadata() {};
 
     //
     // Serialization routine