diff --git a/source/LibMultiSense/details/dispatch.cc b/source/LibMultiSense/details/dispatch.cc index c232e6d8..746c3c08 100644 --- a/source/LibMultiSense/details/dispatch.cc +++ b/source/LibMultiSense/details/dispatch.cc @@ -628,12 +628,18 @@ void impl::dispatch(utility::BufferStreamWriter& buffer) secondary_app::Header header; - header.frameId = SecondaryApp.frameId; - header.source = SecondaryApp.source | ((uint64_t)SecondaryApp.sourceExtended << 32); - header.timeSeconds = SecondaryApp.timeSeconds; - header.timeMicroSeconds = SecondaryApp.timeMicroSeconds; - header.length = SecondaryApp.length; - header.secondaryAppDataP = SecondaryApp.dataP; + wire::SecondaryAppMetadata * metaP = m_secondaryAppMetaCache.find(SecondaryApp.frameId); + if (metaP == NULL) + break; + + header.frameId = SecondaryApp.frameId; + header.source = SecondaryApp.source | ((uint64_t)SecondaryApp.sourceExtended << 32); + header.timeSeconds = SecondaryApp.timeSeconds; + header.timeMicroSeconds = SecondaryApp.timeMicroSeconds; + header.length = SecondaryApp.length; + header.secondaryAppDataP = SecondaryApp.dataP; + header.secondaryAppMetadataP = metaP->dataP; + header.secondaryAppMetadataLength = metaP->dataLength; dispatchSecondaryApplication(buffer, header); break; } @@ -754,24 +760,6 @@ void impl::dispatch(utility::BufferStreamWriter& buffer) } } -Status impl::findMetadata(wire::FeatureDetectorMeta * f, const int64_t & frameId) -{ - wire::SecondaryAppMetadata * m = m_secondaryAppMetaCache.find(frameId); - - if (NULL == m) { - fprintf(stderr, "%s Unable to find metadata for frame: %ld\n", __func__, frameId); - return Status_Error; - } - - utility::BufferStreamReader stream(reinterpret_cast(m->dataP), m->dataLength); - - wire::FeatureDetectorMeta _f(stream, 1); - memcpy(f, &_f, sizeof(_f)); - - return Status_Ok; -} - - // // Get a UDP assembler for this message type. We are given // the first UDP packet in the stream diff --git a/source/LibMultiSense/details/public.cc b/source/LibMultiSense/details/public.cc index 0d3d6173..31b516f7 100644 --- a/source/LibMultiSense/details/public.cc +++ b/source/LibMultiSense/details/public.cc @@ -555,36 +555,27 @@ Status impl::removeIsolatedCallback(secondary_app::Callback callback) return Status_Error; } -Status impl::secondaryAppDataExtract(feature_detector::Header &header, const uint8_t * data, const size_t len, const int64_t frameId) +Status impl::secondaryAppDataExtract(feature_detector::Header &header, const secondary_app::Header &orig) { - utility::BufferStreamReader stream(data, len); - wire::FeatureDetector featureDetector(stream, 1); //TODO + utility::BufferStreamReader stream(reinterpret_cast(orig.secondaryAppDataP), orig.secondaryAppDataLength); + wire::FeatureDetector featureDetector(stream, 1); //TODO Version check + utility::BufferStreamReader metaStream(reinterpret_cast(orig.secondaryAppMetadataP), orig.secondaryAppMetadataLength); - wire::FeatureDetectorMeta m; - if (findMetadata(&m, frameId) == Status_Ok) - { - fprintf(stderr, "%s Meta data extracted\n", __func__ ); - } - else - { - fprintf(stderr, "%s Error metadata extraction for frame %ld failed\n", __func__, frameId ); - return Status_Error; - } - + wire::FeatureDetectorMeta _meta(metaStream, 1); header.source = featureDetector.source; - header.frameId = m.frameId; - header.timeSeconds = m.timeSeconds; - header.timeNanoSeconds= m.timeNanoSeconds; - header.ptpNanoSeconds = m.ptpNanoSeconds; - header.octaveWidth = m.octaveWidth; - header.octaveHeight = m.octaveHeight; - header.numOctaves = m.numOctaves; - header.scaleFactor = m.scaleFactor; - header.motionStatus = m.motionStatus; - header.averageXMotion = m.averageXMotion; - header.averageYMotion = m.averageYMotion; + header.frameId = _meta.frameId; + header.timeSeconds = _meta.timeSeconds; + header.timeNanoSeconds= _meta.timeNanoSeconds; + header.ptpNanoSeconds = _meta.ptpNanoSeconds; + header.octaveWidth = _meta.octaveWidth; + header.octaveHeight = _meta.octaveHeight; + header.numOctaves = _meta.numOctaves; + header.scaleFactor = _meta.scaleFactor; + header.motionStatus = _meta.motionStatus; + header.averageXMotion = _meta.averageXMotion; + header.averageYMotion = _meta.averageYMotion; header.numFeatures = featureDetector.numFeatures; header.numDescriptors = featureDetector.numDescriptors; diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh b/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh index fc9b5364..4b434628 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh @@ -511,7 +511,7 @@ public: * @return A crl::multisense::Status indicating if the callback deregistration * succeeded or failed */ - virtual Status secondaryAppDataExtract(feature_detector::Header &h, const uint8_t * data, const size_t len, const int64_t frameId) = 0; + virtual Status secondaryAppDataExtract(feature_detector::Header &h, const secondary_app::Header &origh) = 0; /** * Reserve image or lidar data within a isolated callback so it is available diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh index 97322221..a8d84ce2 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh @@ -3102,6 +3102,10 @@ public: uint32_t secondaryAppDataLength; /** A pointer to the secondaryAppData data */ const void *secondaryAppDataP; + /** The metadata length */ + uint32_t secondaryAppMetadataLength; + /** The metadata generic payload*/ + const void *secondaryAppMetadataP; /** * Default Constructor diff --git a/source/LibMultiSense/include/MultiSense/details/channel.hh b/source/LibMultiSense/include/MultiSense/details/channel.hh index 088f2884..bc29688d 100644 --- a/source/LibMultiSense/include/MultiSense/details/channel.hh +++ b/source/LibMultiSense/include/MultiSense/details/channel.hh @@ -104,8 +104,6 @@ public: // // Public API - virtual Status findMetadata (wire::FeatureDetectorMeta * f, const int64_t & frameId); - virtual Status addIsolatedCallback (image::Callback callback, DataSource imageSourceMask, void *userDataP); @@ -142,7 +140,7 @@ public: virtual Status removeIsolatedCallback(secondary_app::Callback callback); // virtual Status removeIsolatedCallback(feature_detector::Callback callback); - virtual Status secondaryAppDataExtract(feature_detector::Header &h, const uint8_t * data, const size_t len, const int64_t frameId); + virtual Status secondaryAppDataExtract(feature_detector::Header &h, const secondary_app::Header &orig); virtual void* reserveCallbackBuffer (); virtual Status releaseCallbackBuffer (void *referenceP); @@ -310,7 +308,7 @@ private: static CRL_CONSTEXPR uint32_t RX_POOL_LARGE_BUFFER_SIZE = (10 * (1024 * 1024)); static CRL_CONSTEXPR uint32_t RX_POOL_LARGE_BUFFER_COUNT = 16; static CRL_CONSTEXPR uint32_t RX_POOL_SMALL_BUFFER_SIZE = (8 * (1024)); - static CRL_CONSTEXPR uint32_t RX_POOL_SMALL_BUFFER_COUNT = 128; + static CRL_CONSTEXPR uint32_t RX_POOL_SMALL_BUFFER_COUNT = 256; static CRL_CONSTEXPR uint32_t MAX_BUFFER_ALLOCATION_RETRIES = 5; static double DEFAULT_ACK_TIMEOUT () { return 0.5; } @@ -460,11 +458,6 @@ private: DepthCache m_imageMetaCache; - // - // A cache of feature detector meta data - - // DepthCache m_featureDetectorMetaCache; - // // A cache of secondary application meta data diff --git a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh index 7e13d70e..c145d79f 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh @@ -82,9 +82,18 @@ public: // // Constructors - SecondaryAppMetadata(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);}; + SecondaryAppMetadata(utility::BufferStreamReader&r, VersionType v) { + mRef = new utility::BufferStreamReader(r); + serialize(r, v); + }; + SecondaryAppMetadata() {}; + ~SecondaryAppMetadata() { + if (mRef) + delete mRef; + }; + // // Serialization routine @@ -107,6 +116,8 @@ public: } } + + utility::BufferStreamReader * mRef; }; #endif // !SENSORPOD_FIRMWARE diff --git a/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc b/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc index 3a312aeb..921849ed 100644 --- a/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc +++ b/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc @@ -351,7 +351,9 @@ void featureDetectorCallback(const secondary_app::Header& header, feature_detector::Header fHeader; UserData *userData = reinterpret_cast(userDataP); - Status s = userData->channelP->secondaryAppDataExtract(fHeader, reinterpret_cast(header.secondaryAppDataP), header.secondaryAppDataLength, header.frameId); + Status s = userData->channelP->secondaryAppDataExtract(fHeader, header); + void * buffer = userData->channelP->reserveCallbackBuffer(); + if (s != Status_Ok) { fprintf(stderr, "%s Error failed extraction\n", __func__ ); @@ -389,6 +391,7 @@ void featureDetectorCallback(const secondary_app::Header& header, std::cout << "timeSeconds: " << fHeader.timeSeconds << "\n"; std::cout << "timeNanoSeconds: " << fHeader.timeNanoSeconds << "\n"; std::cout << "ptpNanoSeconds: " << fHeader.ptpNanoSeconds << "\n"; + userData->channelP->releaseCallbackBuffer(buffer); } } // anonymous