diff --git a/source/LibMultiSense/details/dispatch.cc b/source/LibMultiSense/details/dispatch.cc index 746c3c0..ef1f52d 100644 --- a/source/LibMultiSense/details/dispatch.cc +++ b/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 #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::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", diff --git a/source/LibMultiSense/details/public.cc b/source/LibMultiSense/details/public.cc index 31b516f..895bb67 100644 --- a/source/LibMultiSense/details/public.cc +++ b/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(orig.secondaryAppMetadataP), orig.secondaryAppMetadataLength); - wire::FeatureDetectorMeta _meta(metaStream, 1); header.source = featureDetector.source; diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh index a8d84ce..4ae1125 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh +++ b/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*/ diff --git a/source/LibMultiSense/include/MultiSense/details/channel.hh b/source/LibMultiSense/include/MultiSense/details/channel.hh index bc29688..319fda1 100644 --- a/source/LibMultiSense/include/MultiSense/details/channel.hh +++ b/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; // diff --git a/source/LibMultiSense/include/MultiSense/details/utility/BufferStream.hh b/source/LibMultiSense/include/MultiSense/details/utility/BufferStream.hh index f7ff525..04863a0 100644 --- a/source/LibMultiSense/include/MultiSense/details/utility/BufferStream.hh +++ b/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; }; diff --git a/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc b/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc index 921849e..ab2a026 100644 --- a/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc +++ b/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(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(it->second.featureTime - it->second.imageTime).count() - << "ms\n"; + << " Delta: " + << std::chrono::duration_cast(it->second.featureTime - it->second.imageTime).count() + << "ms\n"; userData->elapsedTime.erase(it); } }