Skip to content

Commit

Permalink
Capture metadata buffer reference, auto release in depth cache pop
Browse files Browse the repository at this point in the history
  • Loading branch information
@psmithcrl
psmithcrl committed Nov 18, 2024
1 parent 2861b0e commit b9f0488
Show file tree
Hide file tree
Showing 7 changed files with 51 additions and 61 deletions.
36 changes: 12 additions & 24 deletions source/LibMultiSense/details/dispatch.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down Expand Up @@ -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<uint8_t *>(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
Expand Down
41 changes: 16 additions & 25 deletions source/LibMultiSense/details/public.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<const uint8_t*>(orig.secondaryAppDataP), orig.secondaryAppDataLength);
wire::FeatureDetector featureDetector(stream, 1); //TODO Version check

utility::BufferStreamReader metaStream(reinterpret_cast<const uint8_t *>(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;

Expand Down
2 changes: 1 addition & 1 deletion source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
4 changes: 4 additions & 0 deletions source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
11 changes: 2 additions & 9 deletions source/LibMultiSense/include/MultiSense/details/channel.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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; }
Expand Down Expand Up @@ -460,11 +458,6 @@ private:

DepthCache<int64_t, wire::ImageMeta> m_imageMetaCache;

//
// A cache of feature detector meta data

// DepthCache<int64_t, wire::FeatureDetectorMeta> m_featureDetectorMetaCache;

//
// A cache of secondary application meta data

Expand Down
13 changes: 12 additions & 1 deletion source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand All @@ -107,6 +116,8 @@ public:
}

}

utility::BufferStreamReader * mRef;
};

#endif // !SENSORPOD_FIRMWARE
Expand Down
5 changes: 4 additions & 1 deletion source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -351,7 +351,9 @@ void featureDetectorCallback(const secondary_app::Header& header,

feature_detector::Header fHeader;
UserData *userData = reinterpret_cast<UserData*>(userDataP);
Status s = userData->channelP->secondaryAppDataExtract(fHeader, reinterpret_cast<const uint8_t*>(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__ );
Expand Down Expand Up @@ -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
Expand Down

0 comments on commit b9f0488

Please sign in to comment.