Skip to content

Commit

Permalink
Some changes to support converting generic payload to defined payload
Browse files Browse the repository at this point in the history
  • Loading branch information
@psmithcrl
psmithcrl committed Nov 12, 2024
1 parent 214902b commit fd7ce17
Show file tree
Hide file tree
Showing 6 changed files with 158 additions and 99 deletions.
60 changes: 18 additions & 42 deletions source/LibMultiSense/details/dispatch.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -624,7 +624,6 @@ 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;
Expand All @@ -638,49 +637,8 @@ void impl::dispatch(utility::BufferStreamWriter& buffer)
dispatchSecondaryApplication(buffer, header);
break;
}
// case MSG_ID(wire::FeatureDetector::ID):
// {
// wire::FeatureDetector featureDetector(stream, version);
//
// const wire::FeatureDetectorMeta * metaP = m_featureDetectorMetaCache.find(featureDetector.frameId);
// if (NULL == metaP)
// break;
//
// feature_detector::Header header;
// header.source = featureDetector.source | ((uint64_t)featureDetector.sourceExtended << 32);
// header.frameId = metaP->frameId;
// header.timeSeconds = metaP->timeSeconds;
// header.timeNanoSeconds= metaP->timeNanoSeconds;
// header.ptpNanoSeconds = metaP->ptpNanoSeconds;
// header.octaveWidth = metaP->octaveWidth;
// header.octaveHeight = metaP->octaveHeight;
// header.numOctaves = metaP->numOctaves;
// header.scaleFactor = metaP->scaleFactor;
// header.motionStatus = metaP->motionStatus;
// header.averageXMotion = metaP->averageXMotion;
// header.averageYMotion = metaP->averageYMotion;
// header.numFeatures = featureDetector.numFeatures;
// header.numDescriptors = featureDetector.numDescriptors;
//
// const size_t startDescriptor=featureDetector.numFeatures*sizeof(wire::Feature);
//
// uint8_t * dataP = reinterpret_cast<uint8_t *>(featureDetector.dataP);
// for (size_t i = 0; i < featureDetector.numFeatures; i++) {
// feature_detector::Feature f = *reinterpret_cast<feature_detector::Feature *>(dataP + (i * sizeof(wire::Feature)));
// header.features.push_back(f);
// }
//
// for (size_t j = 0;j < featureDetector.numDescriptors; j++) {
// feature_detector::Descriptor d = *reinterpret_cast<feature_detector::Descriptor *>(dataP + (startDescriptor + (j * sizeof(wire::Descriptor))));
// header.descriptors.push_back(d);
// }
//
// dispatchFeatureDetections(header);
// break;
// }
case MSG_ID(wire::SecondaryAppMetadata::ID):
{
printf("[%s] secondaryAppMetaData\n", __func__ );
wire::SecondaryAppMetadata *metaP = new (std::nothrow) wire::SecondaryAppMetadata(stream, version);

if (NULL == metaP)
Expand Down Expand Up @@ -796,6 +754,24 @@ 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
79 changes: 52 additions & 27 deletions source/LibMultiSense/details/public.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -123,6 +123,8 @@
#include "MultiSense/details/wire/SecondaryAppRegisteredAppsMessage.hh"
#include "MultiSense/details/wire/SecondaryAppActivateMessage.hh"

#include "MultiSense/details/utility/BufferStream.hh"

namespace crl {
namespace multisense {
namespace details {
Expand Down Expand Up @@ -553,33 +555,56 @@ Status impl::removeIsolatedCallback(secondary_app::Callback callback)
return Status_Error;
}

//
// 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::secondaryAppDataExtract(feature_detector::Header &header, const uint8_t * data, const size_t len, const int64_t frameId)
{
utility::BufferStreamReader stream(data, len);
wire::FeatureDetector featureDetector(stream, 1); //TODO


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;
}

header.source = featureDetector.source | ((uint64_t)featureDetector.sourceExtended << 32);
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.numFeatures = featureDetector.numFeatures;
header.numDescriptors = featureDetector.numDescriptors;

const size_t startDescriptor=featureDetector.numFeatures*sizeof(wire::Feature);

uint8_t * dataP = reinterpret_cast<uint8_t *>(featureDetector.dataP);
for (size_t i = 0; i < featureDetector.numFeatures; i++) {
feature_detector::Feature f = *reinterpret_cast<feature_detector::Feature *>(dataP + (i * sizeof(wire::Feature)));
header.features.push_back(f);
}

for (size_t j = 0;j < featureDetector.numDescriptors; j++) {
feature_detector::Descriptor d = *reinterpret_cast<feature_detector::Descriptor *>(dataP + (startDescriptor + (j * sizeof(wire::Descriptor))));
header.descriptors.push_back(d);
}

return Status_Ok;
}




//
// Reserve the current callback buffer being used in a dispatch thread
Expand Down
13 changes: 13 additions & 0 deletions source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -500,6 +500,19 @@ public:
*/
virtual Status removeIsolatedCallback(secondary_app::Callback callback) = 0;

/**
* Provides an interface to convert generic secondary app data to usable feature detector data
*
* @param h the application defined feature detector header
* @param data the generic payload from the secondary application
* @param len the length of the generic payload
* @param frameId the frameId for metadata lookup
*
* @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;

/**
* Reserve image or lidar data within a isolated callback so it is available
* after the callback returns.
Expand Down
5 changes: 5 additions & 0 deletions source/LibMultiSense/include/MultiSense/details/channel.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,7 @@
#include "MultiSense/details/wire/Protocol.hh"
#include "MultiSense/details/wire/ImageMetaMessage.hh"
#include "MultiSense/details/wire/SecondaryAppMetaMessage.hh"
#include "MultiSense/details/wire/FeatureDetectorMetaMessage.hh"
#include "MultiSense/details/wire/StatusResponseMessage.hh"
#include "MultiSense/details/wire/PtpStatusResponseMessage.hh"
#include "MultiSense/details/wire/VersionResponseMessage.hh"
Expand Down Expand Up @@ -103,6 +104,8 @@ 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 @@ -139,6 +142,8 @@ 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 void* reserveCallbackBuffer ();
virtual Status releaseCallbackBuffer (void *referenceP);

Expand Down
2 changes: 2 additions & 0 deletions source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -99,6 +99,8 @@ public:
message.write(dataP, dataLength);

} else {
message & frameId;
message & dataLength;

dataP = message.peek();
message.seek(message.tell() + dataLength);
Expand Down
98 changes: 68 additions & 30 deletions source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -345,43 +345,50 @@ void imageCallback(const image::Header& header,
std::cerr << "failed to get histogram for frame " << header.frameId << std::endl;
}

void featureDetectorCallback(const feature_detector::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, reinterpret_cast<const uint8_t*>(header.secondaryAppDataP), header.secondaryAppDataLength, header.frameId);
if (s != Status_Ok)
{
fprintf(stderr, "%s Error failed extraction\n", __func__ );
return;
}

if ((header.source == Source_Feature_Left)
|| (header.source == Source_Feature_Rectified_Left)) {
if ((fHeader.source == Source_Feature_Left)
|| (fHeader.source == Source_Feature_Rectified_Left)) {

auto it = userData->elapsedTime.find(header.frameId);
auto it = userData->elapsedTime.find(fHeader.frameId);
if (it == userData->elapsedTime.end()) {
std::cout << "Unexpected result, image not yet received for frame: " << header.frameId << "\n";
std::cout << "Unexpected result, image not yet received for frame: " << fHeader.frameId << "\n";
featureDetectionTime t;
t.featureTime = std::chrono::system_clock::now();
userData->elapsedTime.insert(std::pair<int64_t, featureDetectionTime>(header.frameId, t));
userData->elapsedTime.insert(std::pair<int64_t, featureDetectionTime>(fHeader.frameId, t));
}
else
{
it->second.featureTime = std::chrono::system_clock::now();
std::cout << "Feature received after image " << header.frameId
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";
userData->elapsedTime.erase(it);
}
}
std::cout << "Source: " << header.source << "\n";
std::cout << "Frame: " << header.frameId << "\n";
std::cout << "Motion X: " << header.averageXMotion << "\n";
std::cout << "Motion Y: " << header.averageYMotion << "\n";
std::cout << "Number of features: " << header.numFeatures << "\n";
std::cout << "Number of descriptors: " << header.numDescriptors << "\n";
std::cout << "Octave Width: " << header.octaveWidth << "\n";
std::cout << "Octave Height: " << header.octaveHeight << "\n";
std::cout << "timeSeconds: " << header.timeSeconds << "\n";
std::cout << "timeNanoSeconds: " << header.timeNanoSeconds << "\n";
std::cout << "ptpNanoSeconds: " << header.ptpNanoSeconds << "\n";
std::cout << "Source: " << fHeader.source << "\n";
std::cout << "Frame: " << fHeader.frameId << "\n";
std::cout << "Motion X: " << fHeader.averageXMotion << "\n";
std::cout << "Motion Y: " << fHeader.averageYMotion << "\n";
std::cout << "Number of features: " << fHeader.numFeatures << "\n";
std::cout << "Number of descriptors: " << fHeader.numDescriptors << "\n";
std::cout << "Octave Width: " << fHeader.octaveWidth << "\n";
std::cout << "Octave Height: " << fHeader.octaveHeight << "\n";
std::cout << "timeSeconds: " << fHeader.timeSeconds << "\n";
std::cout << "timeNanoSeconds: " << fHeader.timeNanoSeconds << "\n";
std::cout << "ptpNanoSeconds: " << fHeader.ptpNanoSeconds << "\n";
}

} // anonymous
Expand Down Expand Up @@ -496,6 +503,37 @@ int main(int argc,
}

{

system::SecondaryAppRegisteredApps s;
status = channelP->getRegisteredApps(s);
if (Status_Ok != status)
{
std::cerr << "Error failed to get registered apps\n";
return -2;
}

status = channelP->secondaryAppActivate(s.apps[0].appName);
if (Status_Ok != status)
{
std::cerr << "Error failed to activate app " << s.apps[0].appName;
return -2;
}

fprintf(stderr, "%s got registered app: %s activated\n", __func__, s.apps[0].appName.c_str() );

system::FeatureDetectorConfig c;
c.setNumberOfFeatures(5000);
c.setGrouping(1);
c.setMotion(0);
status = channelP->setFeatureDetectorConfig(c);
if (Status_Ok != status)
{
std::cerr << "Error failed to set featureDetectorConfig apps\n";
return -2;
}

std::cout << "Successfully Configured Feature Detector\n";

system::FeatureDetectorConfig fcfg;

status = channelP->getFeatureDetectorConfig(fcfg);
Expand Down Expand Up @@ -585,18 +623,18 @@ int main(int argc,
system::StatusMessage statusMessage;
status = channelP->getDeviceStatus(statusMessage);

if (Status_Ok == status) {
std::cout << "Uptime: " << statusMessage.uptime << ", " <<
"SystemOk: " << statusMessage.systemOk << ", " <<
"FPGA Temp: " << statusMessage.fpgaTemperature << ", " <<
"Left Imager Temp: " << statusMessage.leftImagerTemperature << ", " <<
"Right Imager Temp: " << statusMessage.rightImagerTemperature << ", " <<
"Input Voltage: " << statusMessage.inputVoltage << ", " <<
"Input Current: " << statusMessage.inputCurrent << ", " <<
"FPGA Power: " << statusMessage.fpgaPower << ", " <<
"Logic Power: " << statusMessage.logicPower << ", " <<
"Imager Power: " << statusMessage.imagerPower << std::endl;
}
// if (Status_Ok == status) {
// std::cout << "Uptime: " << statusMessage.uptime << ", " <<
// "SystemOk: " << statusMessage.systemOk << ", " <<
// "FPGA Temp: " << statusMessage.fpgaTemperature << ", " <<
// "Left Imager Temp: " << statusMessage.leftImagerTemperature << ", " <<
// "Right Imager Temp: " << statusMessage.rightImagerTemperature << ", " <<
// "Input Voltage: " << statusMessage.inputVoltage << ", " <<
// "Input Current: " << statusMessage.inputCurrent << ", " <<
// "FPGA Power: " << statusMessage.fpgaPower << ", " <<
// "Logic Power: " << statusMessage.logicPower << ", " <<
// "Imager Power: " << statusMessage.imagerPower << std::endl;
// }

usleep(1000000);
}
Expand Down

0 comments on commit fd7ce17

Please sign in to comment.