diff --git a/source/LibMultiSense/details/channel.cc b/source/LibMultiSense/details/channel.cc index bd146d9..634db66 100644 --- a/source/LibMultiSense/details/channel.cc +++ b/source/LibMultiSense/details/channel.cc @@ -79,7 +79,7 @@ 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_secondaryAppMetaCache(SECONDARY_APP_META_CACHE_DEPTH), m_udpAssemblerMap(), m_dispatchLock(), m_streamLock(), @@ -92,7 +92,7 @@ impl::impl(const std::string& address, const RemoteHeadChannel& cameraId, const m_ppsListeners(), m_imuListeners(), m_compressedImageListeners(), - m_featureDetectorListeners(), + m_secondaryAppListeners(), m_watch(), m_messages(), m_streamsEnabled(0), @@ -264,12 +264,11 @@ void impl::cleanup() itc != m_compressedImageListeners.end(); ++ itc) delete *itc; - std::list::const_iterator itf; - for(itf = m_featureDetectorListeners.begin(); - itf != m_featureDetectorListeners.end(); - ++ itf) - delete *itf; - + std::list::const_iterator its; + for(its = m_secondaryAppListeners.begin(); + its != m_secondaryAppListeners.end(); + its ++) + delete *its; BufferPool::const_iterator it; for(it = m_rxLargeBufferPool.begin(); it != m_rxLargeBufferPool.end(); @@ -284,8 +283,8 @@ void impl::cleanup() m_lidarListeners.clear(); m_ppsListeners.clear(); m_imuListeners.clear(); + m_secondaryAppListeners.clear(); m_compressedImageListeners.clear(); - m_featureDetectorListeners.clear(); m_rxLargeBufferPool.clear(); m_rxSmallBufferPool.clear(); @@ -455,35 +454,29 @@ wire::SourceType impl::sourceApiToWire(DataSource mask) { wire::SourceType wire_mask = 0; - if (mask & Source_Raw_Left) wire_mask |= wire::SOURCE_RAW_LEFT; - if (mask & Source_Raw_Right) wire_mask |= wire::SOURCE_RAW_RIGHT; - if (mask & Source_Raw_Aux) wire_mask |= wire::SOURCE_RAW_AUX; - if (mask & Source_Luma_Left) wire_mask |= wire::SOURCE_LUMA_LEFT; - if (mask & Source_Luma_Right) wire_mask |= wire::SOURCE_LUMA_RIGHT; - if (mask & Source_Luma_Aux) wire_mask |= wire::SOURCE_LUMA_AUX; - if (mask & Source_Luma_Rectified_Left) wire_mask |= wire::SOURCE_LUMA_RECT_LEFT; - if (mask & Source_Luma_Rectified_Right) wire_mask |= wire::SOURCE_LUMA_RECT_RIGHT; - if (mask & Source_Luma_Rectified_Aux) wire_mask |= wire::SOURCE_LUMA_RECT_AUX; - if (mask & Source_Chroma_Left) wire_mask |= wire::SOURCE_CHROMA_LEFT; - if (mask & Source_Chroma_Right) wire_mask |= wire::SOURCE_CHROMA_RIGHT; - if (mask & Source_Chroma_Rectified_Aux) wire_mask |= wire::SOURCE_CHROMA_RECT_AUX; - if (mask & Source_Chroma_Aux) wire_mask |= wire::SOURCE_CHROMA_AUX; - if (mask & Source_Disparity) wire_mask |= wire::SOURCE_DISPARITY; - if (mask & Source_Disparity_Right) wire_mask |= wire::SOURCE_DISPARITY_RIGHT; - if (mask & Source_Disparity_Aux) wire_mask |= wire::SOURCE_DISPARITY_AUX; - if (mask & Source_Disparity_Cost) wire_mask |= wire::SOURCE_DISPARITY_COST; - if (mask & Source_Jpeg_Left) wire_mask |= wire::SOURCE_JPEG_LEFT; - if (mask & Source_Rgb_Left) wire_mask |= wire::SOURCE_RGB_LEFT; - if (mask & Source_Feature_Left) wire_mask |= wire::SOURCE_FEATURE_LEFT; - if (mask & Source_Feature_Right) wire_mask |= wire::SOURCE_FEATURE_RIGHT; - if (mask & Source_Feature_Aux) wire_mask |= wire::SOURCE_FEATURE_AUX; - if (mask & Source_Feature_Rectified_Left) wire_mask |= wire::SOURCE_FEATURE_RECTIFIED_LEFT; - if (mask & Source_Feature_Rectified_Right)wire_mask |= wire::SOURCE_FEATURE_RECTIFIED_RIGHT; - if (mask & Source_Feature_Rectified_Aux) wire_mask |= wire::SOURCE_FEATURE_RECTIFIED_AUX; - if (mask & Source_Lidar_Scan) wire_mask |= wire::SOURCE_LIDAR_SCAN; - if (mask & Source_Imu) wire_mask |= wire::SOURCE_IMU; - if (mask & Source_Pps) wire_mask |= wire::SOURCE_PPS; - if (mask & Source_Compressed_Left) wire_mask |= wire::SOURCE_COMPRESSED_LEFT; + if (mask & Source_Raw_Left) wire_mask |= wire::SOURCE_RAW_LEFT; + if (mask & Source_Raw_Right) wire_mask |= wire::SOURCE_RAW_RIGHT; + if (mask & Source_Raw_Aux) wire_mask |= wire::SOURCE_RAW_AUX; + if (mask & Source_Luma_Left) wire_mask |= wire::SOURCE_LUMA_LEFT; + if (mask & Source_Luma_Right) wire_mask |= wire::SOURCE_LUMA_RIGHT; + if (mask & Source_Luma_Aux) wire_mask |= wire::SOURCE_LUMA_AUX; + if (mask & Source_Luma_Rectified_Left) wire_mask |= wire::SOURCE_LUMA_RECT_LEFT; + if (mask & Source_Luma_Rectified_Right) wire_mask |= wire::SOURCE_LUMA_RECT_RIGHT; + if (mask & Source_Luma_Rectified_Aux) wire_mask |= wire::SOURCE_LUMA_RECT_AUX; + if (mask & Source_Chroma_Left) wire_mask |= wire::SOURCE_CHROMA_LEFT; + if (mask & Source_Chroma_Right) wire_mask |= wire::SOURCE_CHROMA_RIGHT; + if (mask & Source_Chroma_Rectified_Aux) wire_mask |= wire::SOURCE_CHROMA_RECT_AUX; + if (mask & Source_Chroma_Aux) wire_mask |= wire::SOURCE_CHROMA_AUX; + if (mask & Source_Disparity) wire_mask |= wire::SOURCE_DISPARITY; + if (mask & Source_Disparity_Right) wire_mask |= wire::SOURCE_DISPARITY_RIGHT; + if (mask & Source_Disparity_Aux) wire_mask |= wire::SOURCE_DISPARITY_AUX; + if (mask & Source_Disparity_Cost) wire_mask |= wire::SOURCE_DISPARITY_COST; + if (mask & Source_Jpeg_Left) wire_mask |= wire::SOURCE_JPEG_LEFT; + if (mask & Source_Rgb_Left) wire_mask |= wire::SOURCE_RGB_LEFT; + if (mask & Source_Lidar_Scan) wire_mask |= wire::SOURCE_LIDAR_SCAN; + if (mask & Source_Imu) wire_mask |= wire::SOURCE_IMU; + if (mask & Source_Pps) wire_mask |= wire::SOURCE_PPS; + if (mask & Source_Compressed_Left) wire_mask |= wire::SOURCE_COMPRESSED_LEFT; if (mask & Source_Compressed_Rectified_Left) wire_mask |= wire::SOURCE_COMPRESSED_RECTIFIED_LEFT; if (mask & Source_Compressed_Right) wire_mask |= wire::SOURCE_COMPRESSED_RIGHT; if (mask & Source_Compressed_Rectified_Right) wire_mask |= wire::SOURCE_COMPRESSED_RECTIFIED_RIGHT; @@ -492,6 +485,12 @@ wire::SourceType impl::sourceApiToWire(DataSource mask) if (mask & Source_Ground_Surface_Spline_Data) wire_mask |= wire::SOURCE_GROUND_SURFACE_SPLINE_DATA; if (mask & Source_Ground_Surface_Class_Image) wire_mask |= wire::SOURCE_GROUND_SURFACE_CLASS_IMAGE; if (mask & Source_AprilTag_Detections) wire_mask |= wire::SOURCE_APRILTAG_DETECTIONS; + if (mask & Source_Secondary_App_Data_0) wire_mask |= wire::SOURCE_SECONDARY_APP_DATA_0; + if (mask & Source_Secondary_App_Data_1) wire_mask |= wire::SOURCE_SECONDARY_APP_DATA_1; + if (mask & Source_Secondary_App_Data_2) wire_mask |= wire::SOURCE_SECONDARY_APP_DATA_2; + if (mask & Source_Secondary_App_Data_3) wire_mask |= wire::SOURCE_SECONDARY_APP_DATA_3; + if (mask & Source_Secondary_App_Data_4) wire_mask |= wire::SOURCE_SECONDARY_APP_DATA_4; + if (mask & Source_Secondary_App_Data_5) wire_mask |= wire::SOURCE_SECONDARY_APP_DATA_5; return wire_mask; } @@ -519,12 +518,6 @@ DataSource impl::sourceWireToApi(wire::SourceType mask) if (mask & wire::SOURCE_DISPARITY_COST) api_mask |= Source_Disparity_Cost; if (mask & wire::SOURCE_JPEG_LEFT) api_mask |= Source_Jpeg_Left; if (mask & wire::SOURCE_RGB_LEFT) api_mask |= Source_Rgb_Left; - if (mask & wire::SOURCE_FEATURE_LEFT) api_mask |= Source_Feature_Left; - if (mask & wire::SOURCE_FEATURE_RIGHT) api_mask |= Source_Feature_Right; - if (mask & wire::SOURCE_FEATURE_AUX) api_mask |= Source_Feature_Aux; - if (mask & wire::SOURCE_FEATURE_RECTIFIED_LEFT) api_mask |= Source_Feature_Rectified_Left; - if (mask & wire::SOURCE_FEATURE_RECTIFIED_RIGHT) api_mask |= Source_Feature_Rectified_Right; - if (mask & wire::SOURCE_FEATURE_RECTIFIED_AUX) api_mask |= Source_Feature_Rectified_Aux; if (mask & wire::SOURCE_LIDAR_SCAN) api_mask |= Source_Lidar_Scan; if (mask & wire::SOURCE_IMU) api_mask |= Source_Imu; if (mask & wire::SOURCE_PPS) api_mask |= Source_Pps; @@ -537,6 +530,12 @@ DataSource impl::sourceWireToApi(wire::SourceType mask) if (mask & wire::SOURCE_COMPRESSED_RECTIFIED_RIGHT) api_mask |= Source_Compressed_Rectified_Right; if (mask & wire::SOURCE_COMPRESSED_AUX) api_mask |= Source_Compressed_Aux; if (mask & wire::SOURCE_COMPRESSED_RECTIFIED_AUX) api_mask |= Source_Compressed_Rectified_Aux; + if (mask & wire::SOURCE_SECONDARY_APP_DATA_0) api_mask |= Source_Secondary_App_Data_0; + if (mask & wire::SOURCE_SECONDARY_APP_DATA_1) api_mask |= Source_Secondary_App_Data_1; + if (mask & wire::SOURCE_SECONDARY_APP_DATA_2) api_mask |= Source_Secondary_App_Data_2; + if (mask & wire::SOURCE_SECONDARY_APP_DATA_3) api_mask |= Source_Secondary_App_Data_3; + if (mask & wire::SOURCE_SECONDARY_APP_DATA_4) api_mask |= Source_Secondary_App_Data_4; + if (mask & wire::SOURCE_SECONDARY_APP_DATA_5) api_mask |= Source_Secondary_App_Data_5; return api_mask; } diff --git a/source/LibMultiSense/details/dispatch.cc b/source/LibMultiSense/details/dispatch.cc index 485a63b..619dff1 100644 --- a/source/LibMultiSense/details/dispatch.cc +++ b/source/LibMultiSense/details/dispatch.cc @@ -84,12 +84,12 @@ #include "MultiSense/details/wire/GroundSurfaceModel.hh" #include "MultiSense/details/wire/ApriltagDetections.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 "MultiSense/details/wire/SecondaryAppDataMessage.hh" +#include "MultiSense/details/wire/SecondaryAppControlMessage.hh" +#include "MultiSense/details/wire/SecondaryAppConfigMessage.hh" +#include "MultiSense/details/wire/SecondaryAppActivateMessage.hh" +#include "MultiSense/details/wire/SecondaryAppGetRegisteredAppsMessage.hh" +#include "MultiSense/details/wire/SecondaryAppRegisteredAppsMessage.hh" #include @@ -245,21 +245,22 @@ void impl::dispatchAprilTagDetections(apriltag::Header& header) } // -// Publish a feature detection event +// Publish Secondary App Data -void impl::dispatchFeatureDetections(feature_detector::Header& header) +void impl::dispatchSecondaryApplication(utility::BufferStream& buffer, + secondary_app::Header& header) { utility::ScopedLock lock(m_dispatchLock); - std::list::const_iterator it; + std::list::const_iterator it; - for(it = m_featureDetectorListeners.begin(); - it != m_featureDetectorListeners.end(); - ++ it) - (*it)->dispatch(header); + for(it = m_secondaryAppListeners.begin(); + it != m_secondaryAppListeners.end(); + it ++) + (*it)->dispatch(buffer, header); utility::ScopedLock statsLock(m_statisticsLock); - m_channelStatistics.numDispatchedFeatureDetections++; + m_channelStatistics.numDispatchedSecondary++; } @@ -597,54 +598,35 @@ void impl::dispatch(utility::BufferStreamWriter& buffer) dispatchAprilTagDetections(header); break; } - case MSG_ID(wire::FeatureDetector::ID): + case MSG_ID(wire::SecondaryAppData::ID): { - wire::FeatureDetector featureDetector(stream, version); + wire::SecondaryAppData SecondaryApp(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(featureDetector.dataP); - for (size_t i = 0; i < featureDetector.numFeatures; i++) { - feature_detector::Feature f = *reinterpret_cast(dataP + (i * sizeof(wire::Feature))); - header.features.push_back(f); - } + secondary_app::Header header; - for (size_t j = 0;j < featureDetector.numDescriptors; j++) { - feature_detector::Descriptor d = *reinterpret_cast(dataP + (startDescriptor + (j * sizeof(wire::Descriptor)))); - header.descriptors.push_back(d); - } + wire::SecondaryAppMetadata * metaP = m_secondaryAppMetaCache.find(SecondaryApp.frameId); + if (metaP == NULL) + break; - dispatchFeatureDetections(header); + header.frameId = SecondaryApp.frameId; + header.source = SecondaryApp.source | ((uint64_t)SecondaryApp.sourceExtended << 32); + header.timeSeconds = SecondaryApp.timeSeconds; + header.timeMicroSeconds = SecondaryApp.timeMicroSeconds; + header.secondaryAppDataLength = SecondaryApp.length; + header.secondaryAppDataP = SecondaryApp.dataP; + header.secondaryAppMetadataP = metaP->dataP; + header.secondaryAppMetadataLength = metaP->dataLength; + dispatchSecondaryApplication(buffer, header); break; } - case MSG_ID(wire::FeatureDetectorMeta::ID): + case MSG_ID(wire::SecondaryAppMetadata::ID): { - wire::FeatureDetectorMeta *metaP = new (std::nothrow) wire::FeatureDetectorMeta(stream, version); + 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): @@ -718,12 +700,15 @@ void impl::dispatch(utility::BufferStreamWriter& buffer) case MSG_ID(wire::SysExternalCalibration::ID): m_messages.store(wire::SysExternalCalibration(stream, version)); break; + case MSG_ID(wire::SecondaryAppConfig::ID): + m_messages.store(wire::SecondaryAppConfig(stream, version)); + break; + case MSG_ID(wire::SecondaryAppRegisteredApps::ID): + m_messages.store(wire::SecondaryAppRegisteredApps(stream, version)); + break; 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 10f649c..add39cd 100644 --- a/source/LibMultiSense/details/public.cc +++ b/source/LibMultiSense/details/public.cc @@ -106,15 +106,19 @@ #include "MultiSense/details/wire/ImuInfoMessage.hh" #include "MultiSense/details/wire/ImuConfigMessage.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 "MultiSense/details/wire/SysTestMtuMessage.hh" #include "MultiSense/details/wire/SysTestMtuResponseMessage.hh" +#include "MultiSense/details/wire/SecondaryAppConfigMessage.hh" +#include "MultiSense/details/wire/SecondaryAppControlMessage.hh" +#include "MultiSense/details/wire/SecondaryAppGetConfigMessage.hh" +#include "MultiSense/details/wire/SecondaryAppDataMessage.hh" +#include "MultiSense/details/wire/SecondaryAppGetRegisteredAppsMessage.hh" +#include "MultiSense/details/wire/SecondaryAppRegisteredAppsMessage.hh" +#include "MultiSense/details/wire/SecondaryAppActivateMessage.hh" + +#include "MultiSense/details/utility/BufferStream.hh" + namespace crl { namespace multisense { namespace details { @@ -280,27 +284,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 @@ -329,6 +312,27 @@ Status impl::removeIsolatedCallback(image::Callback callback) return Status_Error; } +// +// Adds a new secondarty app listener + +Status impl::addIsolatedCallback(secondary_app::Callback callback, + void *userDataP) +{ + try { + + utility::ScopedLock lock(m_dispatchLock); + m_secondaryAppListeners.push_back(new SecondaryAppListener(callback, + 0, + userDataP, + MAX_USER_SECONDARY_APP_QUEUE_SIZE)); + + } catch (const std::exception& e) { + CRL_DEBUG("exception: %s\n", e.what()); + return Status_Exception; + } + return Status_Ok; +} + // // Removes a lidar listener @@ -498,21 +502,20 @@ Status impl::removeIsolatedCallback(apriltag::Callback callback) } // -// Removes a feature detector listener +// Removes a secondary_app listener -Status impl::removeIsolatedCallback(feature_detector::Callback callback) +Status impl::removeIsolatedCallback(secondary_app::Callback callback) { try { utility::ScopedLock lock(m_dispatchLock); - - std::list::iterator it; - for(it = m_featureDetectorListeners.begin(); - it != m_featureDetectorListeners.end(); - ++ it) { + std::list::iterator it; + for(it = m_secondaryAppListeners.begin(); + it != m_secondaryAppListeners.end(); + it ++) { if ((*it)->callback() == callback) { delete *it; - m_featureDetectorListeners.erase(it); + m_secondaryAppListeners.erase(it); return Status_Ok; } } @@ -1579,29 +1582,100 @@ Status impl::setApriltagParams (const system::ApriltagParams& params) return waitAck(w); } -Status impl::getFeatureDetectorConfig (system::FeatureDetectorConfig & c) +// +// Query camera configuration + +Status impl::getSecondaryAppConfig(system::SecondaryAppConfig& config) +{ + Status status; + wire::SecondaryAppConfig d; + + status = waitData(wire::SecondaryAppGetConfig(), d); + if (Status_Ok != status) + return status; + + + if (d.dataLength >= 1024) + { + std::cerr << "Error: data length exceeds 1024b, truncating\n"; + config.dataLength = 1023; + status = Status_Exception; + } + else + { + config.dataLength = d.dataLength; + } + + memcpy(config.data, d.data, config.dataLength); + + return Status_Ok; +} + + +// +// Set camera configuration +// +// Currently several sensor messages are combined and presented +// to the user as one. + +Status impl::setSecondaryAppConfig(system::SecondaryAppConfig& c) { - wire::FeatureDetectorConfig f; + wire::SecondaryAppControl cmd; - Status status = waitData(wire::FeatureDetectorGetConfig(), f); + c.serialize(); + + if (c.dataLength >= 1024) + { + std::cerr << "Error: data length too large" << std::endl; + return Status_Error; + } + else + { + cmd.dataLength = c.dataLength; + } + + memcpy(cmd.data, c.data, c.dataLength); + + return waitAck(cmd); +} + +Status impl::getRegisteredApps(system::SecondaryAppRegisteredApps& registeredApps) +{ + Status status; + wire::SecondaryAppRegisteredApps d; + + status = waitData(wire::SecondaryAppGetRegisteredApps(), d); if (Status_Ok != status) return status; - c.setNumberOfFeatures(f.numberOfFeatures); - c.setGrouping(f.grouping); - c.setMotion(f.motion); + for (auto app: d.apps) + { + system::SecondaryAppRegisteredApp _a(app.appVersion, app.appName); + registeredApps.apps.push_back(_a); + } return Status_Ok; } -Status impl::setFeatureDetectorConfig (const system::FeatureDetectorConfig & c) + + +// +// Set camera configuration +// +// Currently several sensor messages are combined and presented +// to the user as one. + +Status impl::secondaryAppActivate(const std::string &_name) { - wire::FeatureDetectorControl f; + wire::SecondaryAppActivate cmd(1, _name); - f.numberOfFeatures = c.numberOfFeatures(); - f.grouping = c.grouping(); - f.motion = c.motion(); + return waitAck(cmd); +} - return waitAck(f); +Status impl::secondaryAppDeactivate(const std::string &_name) +{ + wire::SecondaryAppActivate cmd(0, _name); + + return waitAck(cmd); } // diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh b/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh index 16b57c0..a8be58e 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh @@ -350,14 +350,22 @@ public: */ virtual Status addIsolatedCallback(apriltag::Callback callback, void *userDataP=NULL) = 0; - /** - * Add a user defined callback attached to the feature detector stream. + * Add a user defined callback attached to the Secondary result stream. * * Each callback will create a unique internal thread * dedicated to servicing the callback. * - * @param callback A user defined feature_detector::Callback to send Ground + * Secondary data is queued per-callback. For each Secondary + * callback the maximum queue depth is 5 Secondary messages. + * + * Adding multiple callbacks subscribing to the same Secondary data is + * allowed. + * + * Secondary data is stored on the heap and released after returning + * from the callback + * + * @param callback A user defined secondary_App::Callback to send Ground * Surface data to * * @param userDataP A pointer to arbitrary user data. @@ -365,7 +373,7 @@ public: * @return A crl::multisense::Status indicating if the callback registration * succeeded or failed */ - virtual Status addIsolatedCallback(feature_detector::Callback callback, + virtual Status addIsolatedCallback(secondary_app::Callback callback, void *userDataP=NULL) = 0; /** @@ -453,16 +461,15 @@ public: virtual Status removeIsolatedCallback(apriltag::Callback callback) = 0; /** - * Unregister a user defined feature_detector::Callback. This stops the callback - * from receiving feature data + * Unregister a user defined secondary_app::Callback. This stops the callback + * from receiving ground surface data * - * @param callback The user defined feature_detector::Callback to unregister + * @param callback The user defined secondary_app::Callback to unregister * * @return A crl::multisense::Status indicating if the callback deregistration * succeeded or failed */ - - virtual Status removeIsolatedCallback(feature_detector::Callback callback) = 0; + virtual Status removeIsolatedCallback(secondary_app::Callback callback) = 0; /** * Reserve image or lidar data within a isolated callback so it is available @@ -1073,24 +1080,52 @@ public: virtual Status setApriltagParams (const system::ApriltagParams& params) = 0; /** - * Set the feature detector config associated with the MultiSense device + * Get the secondary application config parameters associated with the MultiSense device + * + * @param params The secondary application parameters to send to the secondary application * - * @param params The feature detector parameters to send to the on-camera feature detector + * @return A crl::multisense::Status indicating if the params were successfully queried + */ + virtual Status getSecondaryAppConfig (system::SecondaryAppConfig & c) = 0; + + /** + * Set the secondary application config associated with the MultiSense device + * + * @param params The secondary application parameters to send to the on camera secondary * application * * @return A crl::multisense::Status indicating if the params were successfully set */ - virtual Status setFeatureDetectorConfig (const system::FeatureDetectorConfig& params) = 0; + virtual Status setSecondaryAppConfig (system::SecondaryAppConfig & c) = 0; /** - * Get the feature detector parameters associated with the MultiSense device + * Query the secondary application(s) registered with the MultiSense device * - * @param params The feature detector parameters to send to the on-camera feature detector - * application + * @param params The secondary application(s) supported by this firmware version + * + * @return A crl::multisense::Status indicating if the params were successfully queried + */ + virtual Status getRegisteredApps (system::SecondaryAppRegisteredApps & c) = 0; + + /** + * Activate the secondary application for use with the MultiSense device + * + * @param name the name of the secondary application to activate * * @return A crl::multisense::Status indicating if the params were successfully set */ - virtual Status getFeatureDetectorConfig (system::FeatureDetectorConfig& params) = 0; + virtual Status secondaryAppActivate (const std::string & name) = 0; + + + /** + * Deactivate the secondary application for use with the MultiSense device + * + * @param name the name of the secondary application to deactivate + * + * @return A crl::multisense::Status indicating if the params were successfully set + */ + virtual Status secondaryAppDeactivate(const std::string & s) = 0; + /** * Flash a new FPGA bitstream file to the sensor. diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh index 337ba3c..7b39c08 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh @@ -131,12 +131,6 @@ static CRL_CONSTEXPR DataSource Source_Disparity_Right = (1ull<<11 static CRL_CONSTEXPR DataSource Source_Disparity_Cost = (1ull<<12); static CRL_CONSTEXPR DataSource Source_Jpeg_Left = (1ull<<16); static CRL_CONSTEXPR DataSource Source_Rgb_Left = (1ull<<17); -static CRL_CONSTEXPR DataSource Source_Feature_Left = (1ull<<18); -static CRL_CONSTEXPR DataSource Source_Feature_Right = (1ull<<19); -static CRL_CONSTEXPR DataSource Source_Feature_Aux = (1ull<<32); -static CRL_CONSTEXPR DataSource Source_Feature_Rectified_Left = (1ull<<33); -static CRL_CONSTEXPR DataSource Source_Feature_Rectified_Right = (1ull<<34); -static CRL_CONSTEXPR DataSource Source_Feature_Rectified_Aux = (1ull<<35); static CRL_CONSTEXPR DataSource Source_Ground_Surface_Spline_Data = (1ull<<20); static CRL_CONSTEXPR DataSource Source_Ground_Surface_Class_Image = (1ull<<22); static CRL_CONSTEXPR DataSource Source_AprilTag_Detections = (1ull<<23); @@ -154,6 +148,12 @@ static CRL_CONSTEXPR DataSource Source_Compressed_Aux = (1ull<<14 static CRL_CONSTEXPR DataSource Source_Compressed_Rectified_Left = (1ull<<15); static CRL_CONSTEXPR DataSource Source_Compressed_Rectified_Right = (1ull<<16); static CRL_CONSTEXPR DataSource Source_Compressed_Rectified_Aux = (1ull<<17); +static CRL_CONSTEXPR DataSource Source_Secondary_App_Data_0 = (1ull<<18); +static CRL_CONSTEXPR DataSource Source_Secondary_App_Data_1 = (1ull<<19); +static CRL_CONSTEXPR DataSource Source_Secondary_App_Data_2 = (1ull<<32); +static CRL_CONSTEXPR DataSource Source_Secondary_App_Data_3 = (1ull<<33); +static CRL_CONSTEXPR DataSource Source_Secondary_App_Data_4 = (1ull<<34); +static CRL_CONSTEXPR DataSource Source_Secondary_App_Data_5 = (1ull<<35); /** * Use Roi_Full_Image as the height and width when setting the autoExposureRoi @@ -3018,57 +3018,48 @@ typedef void (*Callback)(const Header& header, } // namespace apriltag -namespace feature_detector { - - - /** The recommended maximum number of features for full resolution camera operation */ - static CRL_CONSTEXPR int RECOMMENDED_MAX_FEATURES_FULL_RES = 5000; - /** The recommended maximum number of features for quarter resolution camera operation */ - static CRL_CONSTEXPR int RECOMMENDED_MAX_FEATURES_QUARTER_RES = 1500; - - - struct Feature { - uint16_t x; - uint16_t y; - uint8_t angle; - uint8_t resp; - uint8_t octave; - uint8_t descriptor; - }; - - struct Descriptor { - uint32_t d[8]; //Descriptor is 32 bytes - }; - - class MULTISENSE_API Header : public HeaderBase { - public: - - DataSource source; - int64_t frameId; - uint32_t timeSeconds; - uint32_t timeNanoSeconds; - int64_t ptpNanoSeconds; - uint16_t octaveWidth; - uint16_t octaveHeight; - uint16_t numOctaves; - uint16_t scaleFactor; - uint16_t motionStatus; - uint16_t averageXMotion; - uint16_t averageYMotion; - uint16_t numFeatures; - uint16_t numDescriptors; - std::vector features; - std::vector descriptors; - }; +namespace secondary_app { - /** - * Function pointer for receiving callbacks for feature_detector data - */ - typedef void (*Callback)(const Header& header, - void *userDataP); +/** + * Class containing Header information for a secondary_app callback. + * + * See crl::multisense::Channel::addIsolatedCallback + */ +class MULTISENSE_API Header : public HeaderBase { +public: + + /** DataSource corresponding to secondaryAppDataP*/ + DataSource source; + /** 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*/ + uint32_t timeSeconds; + /** The time microseconds value corresponding to when the secondaryAppData was captured*/ + uint32_t timeMicroSeconds; + /** The number of frames per second currently streaming from the device */ + float framesPerSecond; + /** The length of the secondaryAppData data stored in secondaryAppDataDataP */ + 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; -} // namespace feature_detector + /** + * Default Constructor + */ + Header() + : source(Source_Unknown) {}; +}; + /** + * Function pointer for receiving callbacks for apriltag data + */ + typedef void (*Callback)(const Header& header, + void *userDataP); +} // namespace secondary_app namespace system { @@ -3941,177 +3932,6 @@ class MULTISENSE_API ApriltagParams { decode_sharpening(0.25) {}; }; -/** - * Example code showing usage of the onboard feature detector. - * Can also reference FeatureDetectorUtility.cc - * - * Example code to set a device's feature detection parameters: - ** \code{.cpp} - * // - * // Instantiate a channel connecting to a sensor at the factory default - * // IP address - * crl::multisense::Channel* channel; - * channel = crl::multisense::Channel::Create("10.66.171.21"); - * - * channel->setMtu(1500); - * - * FeatureDetectorConfig fcfg; - * - * status = channelP->getFeatureDetectorConfig(fcfg); - * if (Status_Ok != status) { - * std::cerr << "Failed to get feature detector config: " << Channel::statusString(status) << std::endl; - * goto clean_out; - * } - * - * if (quarter_res) - * fcfg.setNumberOfFeatures(1500); - * else - * fcfg.setNumberOfFeatures(5000); - * - * fcfg.setGrouping(true); - * fcfg.setMotion(1); - * - * status = channelP->setFeatureDetectorConfig(fcfg); - * if (Status_Ok != status) { - * std::cerr << "Failed to set feature detector config\n"; - * goto clean_out; - * } - * - * // - * // Add Image Callback - * channelP->addIsolatedCallback(imageCallback, Source_Luma_Left|Source_Luma_Right, &userData); - * // - * // Add Feature Callback - * channelP->addIsolatedCallback(featureDetectorCallback, &userData); - * - * // - * // Start streaming - * status = channelP->startStreams((operatingMode.supportedDataSources & Source_Luma_Left) | - * (operatingMode.supportedDataSources & Source_Luma_Right) | - * (operatingMode.supportedDataSources & Source_Feature_Left)| - * (operatingMode.supportedDataSources & Source_Feature_Right)); - * if (Status_Ok != status) { - * std::cerr << "Failed to start streams: " << Channel::statusString(status) << std::endl; - * goto clean_out; - * } * - * // - * // Destroy the channel instance - * crl::multisense::Channel::Destroy(channel); - * \endcode - */ - -class MULTISENSE_API FeatureDetectorConfig { - - private: - - /** - * numberOfFeatures - * The maximum features to be searched for in one image. - * - * Current recommended settings. - * Full Resolution: 5000 Features @5FPS - * Quarter Resolution: 1500 Features @15FPS - */ - uint32_t m_numberOfFeatures; - - /** - * grouping - * Enable/Disable the grouping feature in feaure detection. - * Grouping adds scale invariance to ORB features, by detecting the same - * feature in multiple octaves, and grouping the feature. - * Grouping reduces redundant features and eliminates the need to keep - * track of features referencing the same corner. - * When grouping is enabled, the user should expect less features than - * descriptors, which should result in computationally easier feature matching, - * between consecutive frames. - * Although grouping does come at a slightly reduced framerate, it is - * recommended and verified at the recommended settings. - */ - bool m_grouping; - - /** - * motion - * Enable / disable motion detection in the feature detector. - * When enabled, you can check the averageXMotion, averageYMotion and - * motionStatus of the feaure_detector::header. - * averageXMotion and averageYMotion == 65535 corresponds to a failed - * motion detection for that feature frame. - */ - uint32_t m_motion; - - public: - /** - * Query the maximum number of features applied to the camera feature detector - * - * @return Return the current maximum number of features - */ - uint32_t numberOfFeatures() const { return m_numberOfFeatures; }; - - /** - * Query the status of the feature detector feature grouping - * - * @return Return the current feature grouping status - */ - bool grouping() const { return m_grouping; }; - - /** - * Query the status of the feature detector motion detection - * - * @return Return the current feature detector motion detection status - */ - bool motion() const { return m_motion; }; - - /** - * Set the maximum number of features applied to the camera feature detector. - * Current recommended settings. - * Full Resolution: 5000 Features @5FPS - * Quarter Resolution: 1500 Features @15FPS - * - * @param numberOfFeatures The maximum number of features. - */ - - void setNumberOfFeatures(const uint32_t &numberOfFeatures) { - - if (numberOfFeatures > feature_detector::RECOMMENDED_MAX_FEATURES_FULL_RES) - { - std::cout << "WARNING: The number of features requested is above recommended level!" << '\n'; - std::cout << "If a performance impact is noticed reduce number of features and/or framerate of camera" << '\n'; - std::cout << "The recommended maximum camera settings when using the feature detector is:" << '\n'; - std::cout << "Quarter Res: 15FPS and 1500 Features" << '\n'; - std::cout << "Full Res: 5FPS and 5000 Features" << '\n'; - } - - m_numberOfFeatures = numberOfFeatures; - - }; - - /** - * Set the feature grouping capability the feature detector - * - * @param g The feature grouping to apply to this camera - */ - void setGrouping(const bool &g) { - m_grouping = g; - } - - /** - * Set the feature motion detection capability of the feature detector - * Functions to enable motion detection on Octave 3 - * - * - * @param m The feature detector motion detector. - */ - void setMotion(const uint32_t &m) { - m_motion = m; - } - - /** Default constructor */ - FeatureDetectorConfig(): - m_numberOfFeatures(feature_detector::RECOMMENDED_MAX_FEATURES_QUARTER_RES), - m_grouping(true), - m_motion(1) - {}; -}; /** * PTP status data associated with a specific stamped MultiSense message @@ -4160,7 +3980,8 @@ struct ChannelStatistics numDispatchedImu(0), numDispatchedCompressedImage(0), numDispatchedGroundSurfaceSpline(0), - numDispatchedAprilTagDetections(0) + numDispatchedAprilTagDetections(0), + numDispatchedSecondary(0) { }; @@ -4211,8 +4032,74 @@ 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 { +public: + uint32_t dataLength; + uint8_t data[1024]; + + SecondaryAppConfig(): + dataLength(0), + data() + {} + + SecondaryAppConfig(const uint32_t _length, const uint8_t * _data): + dataLength(_length) + { + if (_length >= 1024) + { + std::cerr << "Error: Secondary Application Config Length > 1024b" << std::endl; + std::cerr << "Truncating data payload to fit in 1024b\n" << std::endl; + dataLength = 1023; + } + + memcpy(data, _data, dataLength); + } + + virtual void serialize( void ) = 0; +}; + +class MULTISENSE_API SecondaryAppActivate { +public: + int activate; + std::string name; + + SecondaryAppActivate(): + activate(0), + name() + { } + + SecondaryAppActivate(const int _activate, const std::string _name): + activate(_activate), + name(_name) + { } +}; + +class MULTISENSE_API SecondaryAppRegisteredApp { +public: + VersionType appVersion; + std::string appName; + + SecondaryAppRegisteredApp( const VersionType v, const std::string n): + appVersion(v), + appName(n) + {} +}; + +class MULTISENSE_API SecondaryAppRegisteredApps { +public: + std::vector apps; + + SecondaryAppRegisteredApps(): + apps() + { } + + SecondaryAppRegisteredApps(const std::vector _apps): + apps(_apps) + { } }; } // namespace system diff --git a/source/LibMultiSense/include/MultiSense/details/channel.hh b/source/LibMultiSense/include/MultiSense/details/channel.hh index e694f0d..9923d4d 100644 --- a/source/LibMultiSense/include/MultiSense/details/channel.hh +++ b/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" @@ -123,7 +123,7 @@ public: virtual Status addIsolatedCallback (apriltag::Callback callback, void *userDataP); - virtual Status addIsolatedCallback (feature_detector::Callback callback, + virtual Status addIsolatedCallback (secondary_app::Callback callback, void *userDataP); virtual Status removeIsolatedCallback(image::Callback callback); @@ -133,7 +133,7 @@ public: virtual Status removeIsolatedCallback(compressed_image::Callback callback); virtual Status removeIsolatedCallback(ground_surface::Callback callback); virtual Status removeIsolatedCallback(apriltag::Callback callback); - virtual Status removeIsolatedCallback(feature_detector::Callback callback); + virtual Status removeIsolatedCallback(secondary_app::Callback callback); virtual void* reserveCallbackBuffer (); virtual Status releaseCallbackBuffer (void *referenceP); @@ -226,8 +226,11 @@ public: uint32_t bufferSize); virtual Status getLocalUdpPort (uint16_t& port); - virtual Status getFeatureDetectorConfig (system::FeatureDetectorConfig & c); - virtual Status setFeatureDetectorConfig (const system::FeatureDetectorConfig & c); + virtual Status getSecondaryAppConfig (system::SecondaryAppConfig & c); + virtual Status setSecondaryAppConfig (system::SecondaryAppConfig & c); + virtual Status getRegisteredApps (system::SecondaryAppRegisteredApps & c); + virtual Status secondaryAppActivate (const std::string & s); + virtual Status secondaryAppDeactivate(const std::string & s); virtual system::ChannelStatistics getStats(); @@ -297,19 +300,19 @@ 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; } static CRL_CONSTEXPR uint32_t DEFAULT_ACK_ATTEMPTS = 5; static CRL_CONSTEXPR uint32_t IMAGE_META_CACHE_DEPTH = 4; - static CRL_CONSTEXPR uint32_t FEATURE_DETECTOR_META_CACHE_DEPTH = 4; + static CRL_CONSTEXPR uint32_t SECONDARY_APP_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 @@ -324,11 +327,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 // @@ -337,9 +346,7 @@ 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; + // // The maximum number of directed streams @@ -445,9 +452,9 @@ private: DepthCache m_imageMetaCache; // - // A cache of feature detector meta data + // A cache of secondary application meta data - DepthCache m_featureDetectorMetaCache; + DepthCache m_secondaryAppMetaCache; // // A map of custom UDP assemblers @@ -492,7 +499,7 @@ private: std::list m_compressedImageListeners; std::list m_groundSurfaceSplineListeners; std::list m_aprilTagDetectionListeners; - std::list m_featureDetectorListeners; + std::list m_secondaryAppListeners; // // A message signal interface @@ -581,7 +588,8 @@ private: compressed_image::Header& header); void dispatchGroundSurfaceSpline(ground_surface::Header& header); void dispatchAprilTagDetections(apriltag::Header& header); - void dispatchFeatureDetections(feature_detector::Header& header); + void dispatchSecondaryApplication(utility::BufferStream& buffer, + secondary_app::Header& header); utility::BufferStreamWriter& findFreeBuffer (uint32_t messageLength); const int64_t& unwrapSequenceId(uint16_t id); diff --git a/source/LibMultiSense/include/MultiSense/details/listeners.hh b/source/LibMultiSense/include/MultiSense/details/listeners.hh index abe63f5..4aa6c5e 100644 --- a/source/LibMultiSense/include/MultiSense/details/listeners.hh +++ b/source/LibMultiSense/include/MultiSense/details/listeners.hh @@ -235,7 +235,7 @@ typedef Listener ImuListen typedef Listener CompressedImageListener; typedef Listener GroundSurfaceSplineListener; typedef Listener AprilTagDetectionListener; -typedef Listener FeatureDetectorListener; +typedef Listener SecondaryAppListener; } // namespace details } // namespace multisense 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/LibMultiSense/include/MultiSense/details/wire/Protocol.hh b/source/LibMultiSense/include/MultiSense/details/wire/Protocol.hh index 356ee6e..0502f33 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/Protocol.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/Protocol.hh @@ -154,48 +154,53 @@ static CRL_CONSTEXPR IdType ID_ACK = 0x0001; // // Commands -static CRL_CONSTEXPR IdType ID_CMD_GET_VERSION = 0x0002; -static CRL_CONSTEXPR IdType ID_CMD_GET_STATUS = 0x0003; -static CRL_CONSTEXPR IdType ID_CMD_CAM_GET_CONFIG = 0x0004; -static CRL_CONSTEXPR IdType ID_CMD_CAM_CONTROL = 0x0007; -static CRL_CONSTEXPR IdType ID_CMD_CAM_GET_HISTORY = 0x0008; -static CRL_CONSTEXPR IdType ID_CMD_CAM_SET_HDR = 0x000b; -static CRL_CONSTEXPR IdType ID_CMD_CAM_SET_RESOLUTION = 0x000c; -static CRL_CONSTEXPR IdType ID_CMD_LIDAR_GET_CONFIG = 0x000d; -static CRL_CONSTEXPR IdType ID_CMD_LIDAR_SET_MOTOR = 0x0010; -static CRL_CONSTEXPR IdType ID_CMD_LED_GET_STATUS = 0x0012; -static CRL_CONSTEXPR IdType ID_CMD_LED_SET = 0x0013; -static CRL_CONSTEXPR IdType ID_CMD_SYS_MTU = 0x0014; -static CRL_CONSTEXPR IdType ID_CMD_SYS_FLASH_OP = 0x0015; -static CRL_CONSTEXPR IdType ID_CMD_SYS_SET_NETWORK = 0x0016; -static CRL_CONSTEXPR IdType ID_CMD_SYS_GET_DEVICE_INFO = 0x0017; -static CRL_CONSTEXPR IdType ID_CMD_SYS_GET_CAMERA_CAL = 0x0018; -static CRL_CONSTEXPR IdType ID_CMD_SYS_GET_LIDAR_CAL = 0x0019; -static CRL_CONSTEXPR IdType ID_CMD_SYS_GET_MTU = 0x001a; -static CRL_CONSTEXPR IdType ID_CMD_SYS_GET_NETWORK = 0x001b; -static CRL_CONSTEXPR IdType ID_CMD_STREAM_CONTROL = 0x001c; -static CRL_CONSTEXPR IdType ID_CMD_SYS_GET_DEVICE_MODES = 0x001d; -static CRL_CONSTEXPR IdType ID_CMD_CAM_SET_TRIGGER_SOURCE = 0x001e; -static CRL_CONSTEXPR IdType ID_CMD_IMU_GET_INFO = 0x001f; -static CRL_CONSTEXPR IdType ID_CMD_IMU_GET_CONFIG = 0x0020; -static CRL_CONSTEXPR IdType ID_CMD_SYS_TEST_MTU = 0x0021; -static CRL_CONSTEXPR IdType ID_CMD_SYS_GET_DIRECTED_STREAMS = 0x0022; -static CRL_CONSTEXPR IdType ID_CMD_SYS_GET_SENSOR_CAL = 0x0023; -static CRL_CONSTEXPR IdType ID_CMD_SYS_GET_EXTERNAL_CAL = 0x0024; -static CRL_CONSTEXPR IdType ID_CMD_LED_GET_SENSOR_STATUS = 0x0025; -static CRL_CONSTEXPR IdType ID_CMD_SYS_SET_TRANSMIT_DELAY = 0x0026; -static CRL_CONSTEXPR IdType ID_CMD_SYS_GET_TRANSMIT_DELAY = 0x0027; -static CRL_CONSTEXPR IdType ID_CMD_SYS_MOTOR_POLL = 0x0028; -static CRL_CONSTEXPR IdType ID_CMD_SYS_SET_PTP = 0x0029; -static CRL_CONSTEXPR IdType ID_CMD_CAM_AUX_CONTROL = 0x002a; -static CRL_CONSTEXPR IdType ID_CMD_CAM_GET_AUX_CONFIG = 0x002b; -static CRL_CONSTEXPR IdType ID_CMD_REMOTE_HEAD_GET_CONFIG = 0x002c; -static CRL_CONSTEXPR IdType ID_CMD_REMOTE_HEAD_CONTROL = 0x002d; -static CRL_CONSTEXPR IdType ID_CMD_GET_PTP_STATUS = 0x002e; -static CRL_CONSTEXPR IdType ID_CMD_SYS_SET_PACKET_DELAY = 0x002f; -static CRL_CONSTEXPR IdType ID_CMD_SYS_GET_PACKET_DELAY = 0x0030; -static CRL_CONSTEXPR IdType ID_CMD_FEATURE_DETECTOR_GET_CONFIG = 0x0031; -static CRL_CONSTEXPR IdType ID_CMD_FEATURE_DETECTOR_CONTROL = 0x0032; +static CRL_CONSTEXPR IdType ID_CMD_GET_VERSION = 0x0002; +static CRL_CONSTEXPR IdType ID_CMD_GET_STATUS = 0x0003; +static CRL_CONSTEXPR IdType ID_CMD_CAM_GET_CONFIG = 0x0004; +static CRL_CONSTEXPR IdType ID_CMD_CAM_CONTROL = 0x0007; +static CRL_CONSTEXPR IdType ID_CMD_CAM_GET_HISTORY = 0x0008; +static CRL_CONSTEXPR IdType ID_CMD_CAM_SET_HDR = 0x000b; +static CRL_CONSTEXPR IdType ID_CMD_CAM_SET_RESOLUTION = 0x000c; +static CRL_CONSTEXPR IdType ID_CMD_LIDAR_GET_CONFIG = 0x000d; +static CRL_CONSTEXPR IdType ID_CMD_LIDAR_SET_MOTOR = 0x0010; +static CRL_CONSTEXPR IdType ID_CMD_LED_GET_STATUS = 0x0012; +static CRL_CONSTEXPR IdType ID_CMD_LED_SET = 0x0013; +static CRL_CONSTEXPR IdType ID_CMD_SYS_MTU = 0x0014; +static CRL_CONSTEXPR IdType ID_CMD_SYS_FLASH_OP = 0x0015; +static CRL_CONSTEXPR IdType ID_CMD_SYS_SET_NETWORK = 0x0016; +static CRL_CONSTEXPR IdType ID_CMD_SYS_GET_DEVICE_INFO = 0x0017; +static CRL_CONSTEXPR IdType ID_CMD_SYS_GET_CAMERA_CAL = 0x0018; +static CRL_CONSTEXPR IdType ID_CMD_SYS_GET_LIDAR_CAL = 0x0019; +static CRL_CONSTEXPR IdType ID_CMD_SYS_GET_MTU = 0x001a; +static CRL_CONSTEXPR IdType ID_CMD_SYS_GET_NETWORK = 0x001b; +static CRL_CONSTEXPR IdType ID_CMD_STREAM_CONTROL = 0x001c; +static CRL_CONSTEXPR IdType ID_CMD_SYS_GET_DEVICE_MODES = 0x001d; +static CRL_CONSTEXPR IdType ID_CMD_CAM_SET_TRIGGER_SOURCE = 0x001e; +static CRL_CONSTEXPR IdType ID_CMD_IMU_GET_INFO = 0x001f; +static CRL_CONSTEXPR IdType ID_CMD_IMU_GET_CONFIG = 0x0020; +static CRL_CONSTEXPR IdType ID_CMD_SYS_TEST_MTU = 0x0021; +static CRL_CONSTEXPR IdType ID_CMD_SYS_GET_DIRECTED_STREAMS = 0x0022; +static CRL_CONSTEXPR IdType ID_CMD_SYS_GET_SENSOR_CAL = 0x0023; +static CRL_CONSTEXPR IdType ID_CMD_SYS_GET_EXTERNAL_CAL = 0x0024; +static CRL_CONSTEXPR IdType ID_CMD_LED_GET_SENSOR_STATUS = 0x0025; +static CRL_CONSTEXPR IdType ID_CMD_SYS_SET_TRANSMIT_DELAY = 0x0026; +static CRL_CONSTEXPR IdType ID_CMD_SYS_GET_TRANSMIT_DELAY = 0x0027; +static CRL_CONSTEXPR IdType ID_CMD_SYS_MOTOR_POLL = 0x0028; +static CRL_CONSTEXPR IdType ID_CMD_SYS_SET_PTP = 0x0029; +static CRL_CONSTEXPR IdType ID_CMD_CAM_AUX_CONTROL = 0x002a; +static CRL_CONSTEXPR IdType ID_CMD_CAM_GET_AUX_CONFIG = 0x002b; +static CRL_CONSTEXPR IdType ID_CMD_REMOTE_HEAD_GET_CONFIG = 0x002c; +static CRL_CONSTEXPR IdType ID_CMD_REMOTE_HEAD_CONTROL = 0x002d; +static CRL_CONSTEXPR IdType ID_CMD_GET_PTP_STATUS = 0x002e; +static CRL_CONSTEXPR IdType ID_CMD_SYS_SET_PACKET_DELAY = 0x002f; +static CRL_CONSTEXPR IdType ID_CMD_SYS_GET_PACKET_DELAY = 0x0030; +static CRL_CONSTEXPR IdType ID_CMD_FEATURE_DETECTOR_GET_CONFIG = 0x0031; +static CRL_CONSTEXPR IdType ID_CMD_FEATURE_DETECTOR_CONTROL = 0x0032; +static CRL_CONSTEXPR IdType ID_CMD_SECONDARY_APP_GET_CONFIG = 0x0031; // Same as ID_CMD_FEATURE_DETECTOR_GET_CONFIG +static CRL_CONSTEXPR IdType ID_CMD_SECONDARY_APP_CONTROL = 0x0032; // Same as ID_CMD_FEATURE_DETECTOR_CONTROL +static CRL_CONSTEXPR IdType ID_CMD_SECONDARY_APP_GET_REGISTERED_APPS = 0x0033; +static CRL_CONSTEXPR IdType ID_CMD_SECONDARY_APP_ACTIVATE = 0x0034; + // // Data @@ -238,6 +243,11 @@ static CRL_CONSTEXPR IdType ID_DATA_SYS_PACKET_DELAY = 0x0127; static CRL_CONSTEXPR IdType ID_DATA_FEATURE_DETECTOR_META = 0x0128; static CRL_CONSTEXPR IdType ID_DATA_FEATURE_DETECTOR = 0x0129; static CRL_CONSTEXPR IdType ID_DATA_FEATURE_DETECTOR_CONFIG = 0x012A; +static CRL_CONSTEXPR IdType ID_DATA_SECONDARY_APP_META = 0x0128; // Same as ID_DATA_FEATURE_DETECTOR_META +static CRL_CONSTEXPR IdType ID_DATA_SECONDARY_APP = 0x0129; // Same as ID_DATA_FEATURE_DETECTOR +static CRL_CONSTEXPR IdType ID_DATA_SECONDARY_APP_CONFIG = 0x012A; // Same as ID_DATA_FEATURE_DETECTOR_CONFIG +static CRL_CONSTEXPR IdType ID_DATA_SECONDARY_APP_REGISTERED_APPS = 0x012B; + // // Data sources @@ -260,12 +270,6 @@ static CRL_CONSTEXPR SourceType SOURCE_DISPARITY_RIGHT = (1ull<<11); static CRL_CONSTEXPR SourceType SOURCE_DISPARITY_COST = (1ull<<12); static CRL_CONSTEXPR SourceType SOURCE_JPEG_LEFT = (1ull<<16); static CRL_CONSTEXPR SourceType SOURCE_RGB_LEFT = (1ull<<17); -static CRL_CONSTEXPR SourceType SOURCE_FEATURE_LEFT = (1ull<<18); -static CRL_CONSTEXPR SourceType SOURCE_FEATURE_RIGHT = (1ull<<19); -static CRL_CONSTEXPR SourceType SOURCE_FEATURE_AUX = (1ull<<32); -static CRL_CONSTEXPR SourceType SOURCE_FEATURE_RECTIFIED_LEFT = (1ull<<33); -static CRL_CONSTEXPR SourceType SOURCE_FEATURE_RECTIFIED_RIGHT = (1ull<<34); -static CRL_CONSTEXPR SourceType SOURCE_FEATURE_RECTIFIED_AUX = (1ull<<35); static CRL_CONSTEXPR SourceType SOURCE_GROUND_SURFACE_SPLINE_DATA = (1ull<<20); static CRL_CONSTEXPR SourceType SOURCE_GROUND_SURFACE_CLASS_IMAGE = (1ull<<22); static CRL_CONSTEXPR SourceType SOURCE_APRILTAG_DETECTIONS = (1ull<<21); @@ -284,6 +288,19 @@ static CRL_CONSTEXPR SourceType SOURCE_COMPRESSED_AUX = (1ull<<14); static CRL_CONSTEXPR SourceType SOURCE_COMPRESSED_RECTIFIED_LEFT = (1ull<<15); static CRL_CONSTEXPR SourceType SOURCE_COMPRESSED_RECTIFIED_RIGHT = (1ull<<16); // same as SOURCE_JPEG_LEFT static CRL_CONSTEXPR SourceType SOURCE_COMPRESSED_RECTIFIED_AUX = (1ull<<17); // same as SOURCE_RGB_LEFT +static CRL_CONSTEXPR SourceType SOURCE_SECONDARY_APP_DATA_0 = (1ull<<18); +static CRL_CONSTEXPR SourceType SOURCE_SECONDARY_APP_DATA_1 = (1ull<<19); +static CRL_CONSTEXPR SourceType SOURCE_SECONDARY_APP_DATA_2 = (1ull<<32); +static CRL_CONSTEXPR SourceType SOURCE_SECONDARY_APP_DATA_3 = (1ull<<33); +static CRL_CONSTEXPR SourceType SOURCE_SECONDARY_APP_DATA_4 = (1ull<<34); +static CRL_CONSTEXPR SourceType SOURCE_SECONDARY_APP_DATA_5 = (1ull<<35); + +static CRL_CONSTEXPR SourceType SOURCE_SECONDARY_APP_DATA = (SOURCE_SECONDARY_APP_DATA_0 | + SOURCE_SECONDARY_APP_DATA_1 | + SOURCE_SECONDARY_APP_DATA_2 | + SOURCE_SECONDARY_APP_DATA_3 | + SOURCE_SECONDARY_APP_DATA_4 | + SOURCE_SECONDARY_APP_DATA_5); static CRL_CONSTEXPR SourceType SOURCE_IMAGES = (SOURCE_RAW_LEFT | SOURCE_RAW_RIGHT | @@ -314,13 +331,6 @@ static CRL_CONSTEXPR SourceType SOURCE_IMAGES = (SOURCE_RAW_LEFT SOURCE_COMPRESSED_RECTIFIED_AUX ); -static CRL_CONSTEXPR SourceType SOURCE_FEATURE_DETECTOR = (SOURCE_FEATURE_LEFT | - SOURCE_FEATURE_RIGHT| - SOURCE_FEATURE_AUX | - SOURCE_FEATURE_RECTIFIED_LEFT | - SOURCE_FEATURE_RECTIFIED_RIGHT | - SOURCE_FEATURE_RECTIFIED_AUX - ); // // Exposure config diff --git a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppActivateMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppActivateMessage.hh new file mode 100644 index 0000000..988cfdd --- /dev/null +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppActivateMessage.hh @@ -0,0 +1,89 @@ +/** + * @file LibMultiSense/SecondaryAppActivateMessage.hh + * + * This message contains a request for camera configuration. + * + * Copyright 2013-2023 + * Carnegie Robotics, LLC + * 4501 Hatfield Street, Pittsburgh, PA 15201 + * http://www.carnegierobotics.com + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Carnegie Robotics, LLC nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CARNEGIE ROBOTICS, LLC BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Significant history (date, user, job code, action): + * 2024-09-19, patrick.smith@carnegierobotics.com, IRAD, Copied from CamGetConfigMessage.hh. + **/ + +#ifndef LibMultiSense_SecondaryAppActivate +#define LibMultiSense_SecondaryAppActivate + +#include "MultiSense/details/utility/Portability.hh" + +namespace crl { +namespace multisense { +namespace details { +namespace wire { + +class SecondaryAppActivate { +public: + static CRL_CONSTEXPR VersionType VERSION = 1; + static CRL_CONSTEXPR IdType ID = ID_CMD_SECONDARY_APP_ACTIVATE; + + + // + // Parameters representing the current camera configuration + int activate; + + std::string name; + + // + // Constructors + + SecondaryAppActivate(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);}; + SecondaryAppActivate() {}; + SecondaryAppActivate(const int i, const std::string & s): + activate(i), + name(s) + {} + + // + // Serialization routine. + + template + void serialize(Archive& message, + const VersionType version) + { + (void) version; + // nothing yet + + message & activate; + + message & name; + } +}; + +}}}} // namespaces + +#endif diff --git a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorGetConfigMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppConfigMessage.hh similarity index 74% rename from source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorGetConfigMessage.hh rename to source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppConfigMessage.hh index d83c1a7..dcab2ab 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorGetConfigMessage.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppConfigMessage.hh @@ -1,13 +1,12 @@ /** - * @file LibMultiSense/FeatureDetectorGetConfigMessage.hh + * @file LibMultiSense/SecondaryAppConfig.hh * - * This message contains the request to get the current feature detector - * configuration. + * This message contains the current camera configuration. * - * Copyright 2013-2024 + * Copyright 2013-2023 * Carnegie Robotics, LLC * 4501 Hatfield Street, Pittsburgh, PA 15201 - * http://www.carnegiearobotics.com + * http://www.carnegierobotics.com * * All rights reserved. * @@ -34,11 +33,11 @@ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * * Significant history (date, user, job code, action): - * 2024-25-01, patrick.smith@carnegierobotics.com, IRAD, Created file. + * 2023-09-19, patrick.smith@carnegierobotics.com, IRAD, Created file. **/ -#ifndef LibMultisense_FeatureDetectorGetConfigMessage -#define LibMultisense_FeatureDetectorGetConfigMessage +#ifndef LibMultiSense_SecondaryAppConfig +#define LibMultiSense_SecondaryAppConfig #include "MultiSense/details/utility/Portability.hh" #include "MultiSense/details/wire/Protocol.hh" @@ -48,19 +47,27 @@ namespace multisense { namespace details { namespace wire { -class FeatureDetectorGetConfig { +class SecondaryAppConfig { public: - static CRL_CONSTEXPR IdType ID = ID_CMD_FEATURE_DETECTOR_GET_CONFIG; + static CRL_CONSTEXPR IdType ID = ID_DATA_SECONDARY_APP_CONFIG; static CRL_CONSTEXPR VersionType VERSION = 1; // // Parameters representing the current camera configuration + uint32_t dataLength; + uint8_t data[1024]; // // Constructors - FeatureDetectorGetConfig(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);}; - FeatureDetectorGetConfig() {}; + SecondaryAppConfig(utility::BufferStreamReader&r, VersionType v) { + serialize(r,v); + }; + + SecondaryAppConfig(): + dataLength(0), + data() + {}; // // Serialization routine @@ -69,8 +76,13 @@ public: void serialize(Archive& message, const VersionType version) { - (void) message; (void) version; + message & dataLength; + + for (size_t i = 0; i < dataLength; i++) + { + message & data[i]; + } } }; diff --git a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorControlMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppControlMessage.hh similarity index 75% rename from source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorControlMessage.hh rename to source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppControlMessage.hh index 0b9c842..aad96f5 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorControlMessage.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppControlMessage.hh @@ -1,12 +1,12 @@ /** - * @file LibMultiSense/FeatureDetectorControlMessage.hh + * @file LibMultiSense/SecondaryAppControl.hh * - * This message contains the current feature detector configuration. + * This message contains the current camera configuration. * - * Copyright 2013-2024 + * Copyright 2013-2023 * Carnegie Robotics, LLC * 4501 Hatfield Street, Pittsburgh, PA 15201 - * http://www.carnegiearobotics.com + * http://www.carnegierobotics.com * * All rights reserved. * @@ -33,11 +33,11 @@ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * * Significant history (date, user, job code, action): - * 2024-25-01, patrick.smith@carnegierobotics.com, IRAD, Created file. + * 2023-09-19, patrick.smith@carnegierobotics.com, IRAD, Created file. **/ -#ifndef LibMultisense_FeatureDetectorControlMessage -#define LibMultisense_FeatureDetectorControlMessage +#ifndef LibMultiSense_SecondaryAppControl +#define LibMultiSense_SecondaryAppControl #include "MultiSense/details/utility/Portability.hh" #include "MultiSense/details/wire/Protocol.hh" @@ -47,25 +47,26 @@ namespace multisense { namespace details { namespace wire { -class FeatureDetectorControl { +class SecondaryAppControl { public: - static CRL_CONSTEXPR IdType ID = ID_CMD_FEATURE_DETECTOR_CONTROL; + static CRL_CONSTEXPR IdType ID = ID_CMD_SECONDARY_APP_CONTROL; static CRL_CONSTEXPR VersionType VERSION = 1; // // Parameters representing the current camera configuration + uint32_t dataLength; - uint32_t numberOfFeatures; + uint8_t data[1024]; - uint32_t grouping; - - uint32_t motion; // // Constructors - FeatureDetectorControl(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);}; - FeatureDetectorControl() {}; + SecondaryAppControl(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);}; + SecondaryAppControl(): + dataLength(0), + data() + {}; // // Serialization routine @@ -74,12 +75,12 @@ public: void serialize(Archive& message, const VersionType version) { - (void) version; - message & numberOfFeatures; - message & grouping; - message & motion; - + message & dataLength; + for (size_t i = 0; i < dataLength; i++) + { + message & data[i]; + } } }; diff --git a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppDataMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppDataMessage.hh new file mode 100644 index 0000000..178feec --- /dev/null +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppDataMessage.hh @@ -0,0 +1,127 @@ +/** + * @file LibMultiSense/SecondarryAppDataMessage.hh + * + * Copyright 2013-2022 + * Carnegie Robotics, LLC + * 4501 Hatfield Street, Pittsburgh, PA 15201 + * http://www.carnegierobotics.com + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Carnegie Robotics, LLC nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CARNEGIE ROBOTICS, LLC BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Significant history (date, user, job code, action): + * 2023-09-19, patrick.smith@carnegierobotics.com, IRAD, created file. + **/ + +#ifndef LibMultiSense_SecondaryAppData +#define LibMultiSense_SecondaryAppData + +#include +#include + +#include "MultiSense/details/utility/Portability.hh" + +namespace crl { +namespace multisense { +namespace details { +namespace wire { + +class WIRE_HEADER_ATTRIBS_ SecondaryAppHeader { +public: + +static CRL_CONSTEXPR IdType ID = ID_DATA_SECONDARY_APP; +static CRL_CONSTEXPR VersionType VERSION = 1; + +#ifdef SENSORPOD_FIRMWARE + IdType id; + VersionType version; +#endif // SENSORPOD_FIRMWARE + + uint32_t source; + uint32_t length; + int64_t frameId; + uint32_t timeSeconds; + uint32_t timeMicroSeconds; + uint32_t sourceExtended; + + SecondaryAppHeader() + : +#ifdef SENSORDPOD_FIRMWARE + id(ID), + version(VERSION), +#endif // SENSORPOD_FIRMWARE + source(0), + length(0), + frameId(0), + timeSeconds(0), + timeMicroSeconds(0) + {}; +}; + +#ifndef SENSORPOD_FIRMWARE + +class SecondaryAppData : public SecondaryAppHeader { +public: + + void *dataP; + + // + // Constructors + + SecondaryAppData(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);}; + SecondaryAppData() : dataP(NULL) {}; + + // + // Serialization routine + + template + void serialize(Archive& message, + const VersionType version) + { + (void) version; + message & source; + message & length; + message & frameId; + message & timeSeconds; + message & timeMicroSeconds; + message & sourceExtended; + + if (typeid(Archive) == typeid(utility::BufferStreamWriter)) { + + message.write(dataP, length); + + } else { + + dataP = message.peek(); + message.seek(message.tell() + length); + } + + } +}; + +#endif // !SENSORPOD_FIRMWARE + +}}}} // namespaces + +#endif diff --git a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppGetConfigMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppGetConfigMessage.hh new file mode 100644 index 0000000..38e32f9 --- /dev/null +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppGetConfigMessage.hh @@ -0,0 +1,75 @@ +/** + * @file LibMultiSense/SecondaryAppGetConfig.hh + * + * This message contains a request for camera configuration. + * + * Copyright 2013-2023 + * Carnegie Robotics, LLC + * 4501 Hatfield Street, Pittsburgh, PA 15201 + * http://www.carnegierobotics.com + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Carnegie Robotics, LLC nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CARNEGIE ROBOTICS, LLC BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Significant history (date, user, job code, action): + * 2023-09-19, patrick.smith@carnegierobotics.com, IRAD, Copied from CamGetConfigMessage.hh. + **/ + +#ifndef LibMultiSense_SecondaryAppGetConfig +#define LibMultiSense_SecondaryAppGetConfig + +#include "MultiSense/details/utility/Portability.hh" + +namespace crl { +namespace multisense { +namespace details { +namespace wire { + +class SecondaryAppGetConfig { +public: + static CRL_CONSTEXPR VersionType VERSION = 1; + static CRL_CONSTEXPR IdType ID = ID_CMD_SECONDARY_APP_GET_CONFIG; + + // + // Constructors + + SecondaryAppGetConfig(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);}; + SecondaryAppGetConfig() {}; + + // + // Serialization routine. + + template + void serialize(Archive& message, + const VersionType version) + { + (void) message; + (void) version; + // nothing yet + } +}; + +}}}} // namespaces + +#endif diff --git a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppGetRegisteredAppsMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppGetRegisteredAppsMessage.hh new file mode 100644 index 0000000..809d01b --- /dev/null +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppGetRegisteredAppsMessage.hh @@ -0,0 +1,75 @@ +/** + * @file LibMultiSense/SecondaryAppGetRegisteredAppsMessage.hh + * + * This message contains a request for camera configuration. + * + * Copyright 2013-2023 + * Carnegie Robotics, LLC + * 4501 Hatfield Street, Pittsburgh, PA 15201 + * http://www.carnegierobotics.com + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Carnegie Robotics, LLC nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CARNEGIE ROBOTICS, LLC BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Significant history (date, user, job code, action): + * 2024-09-10, patrick.smith@carnegierobotics.com, IRAD, Copied from CamGetConfigMessage.hh. + **/ + +#ifndef LibMultiSense_SecondaryAppGetRegisteredApps +#define LibMultiSense_SecondaryAppGetRegisteredApps + +#include "MultiSense/details/utility/Portability.hh" + +namespace crl { +namespace multisense { +namespace details { +namespace wire { + +class SecondaryAppGetRegisteredApps { +public: + static CRL_CONSTEXPR VersionType VERSION = 1; + static CRL_CONSTEXPR IdType ID = ID_CMD_SECONDARY_APP_GET_REGISTERED_APPS; + + // + // Constructors + + SecondaryAppGetRegisteredApps(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);}; + SecondaryAppGetRegisteredApps() {}; + + // + // Serialization routine. + + template + void serialize(Archive& message, + const VersionType version) + { + (void) message; + (void) version; + // nothing yet + } +}; + +}}}} // namespaces + +#endif diff --git a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh new file mode 100644 index 0000000..c145d79 --- /dev/null +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh @@ -0,0 +1,127 @@ +/** + * @file LibMultiSense/SecondaryAppMetaMessage.hh + * + * Copyright 2013-2022 + * Carnegie Robotics, LLC + * 4501 Hatfield Street, Pittsburgh, PA 15201 + * http://www.carnegierobotics.com + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Carnegie Robotics, LLC nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CARNEGIE ROBOTICS, LLC BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Significant history (date, user, job code, action): + * 2024-10-31, patrick.smith@carnegierobotics.com, IRAD, created file. + **/ + +#ifndef LibMultiSense_SecondaryAppMetaData +#define LibMultiSense_SecondaryAppMetaData + +#include +#include + +#include "MultiSense/details/utility/Portability.hh" + +namespace crl { +namespace multisense { +namespace details { +namespace wire { + +class WIRE_HEADER_ATTRIBS_ SecondaryAppMetaHeader { +public: + +static CRL_CONSTEXPR IdType ID = ID_DATA_FEATURE_DETECTOR_META; +static CRL_CONSTEXPR VersionType VERSION = 1; + +#ifdef SENSORPOD_FIRMWARE + IdType id; + VersionType version; +#endif // SENSORPOD_FIRMWARE + + 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) + {}; +}; + +#ifndef SENSORPOD_FIRMWARE + +class SecondaryAppMetadata : public SecondaryAppMetaHeader { +public: + + + // + // Constructors + + SecondaryAppMetadata(utility::BufferStreamReader&r, VersionType v) { + mRef = new utility::BufferStreamReader(r); + serialize(r, v); + }; + + SecondaryAppMetadata() {}; + + ~SecondaryAppMetadata() { + if (mRef) + delete mRef; + }; + + // + // Serialization routine + + template + void serialize(Archive& message, + const VersionType version) + { + (void) version; + + if (typeid(Archive) == typeid(utility::BufferStreamWriter)) { + + message.write(dataP, dataLength); + + } else { + message & frameId; + message & dataLength; + + dataP = message.peek(); + message.seek(message.tell() + dataLength); + } + + } + + utility::BufferStreamReader * mRef; +}; + +#endif // !SENSORPOD_FIRMWARE + +}}}} // namespaces + +#endif diff --git a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppRegisteredAppsMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppRegisteredAppsMessage.hh new file mode 100644 index 0000000..4b9bf18 --- /dev/null +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppRegisteredAppsMessage.hh @@ -0,0 +1,94 @@ +/** + * @file LibMultiSense/SecondaryAppRegisteredAppsMessage.hh + * + * This message contains a request for camera configuration. + * + * Copyright 2013-2023 + * Carnegie Robotics, LLC + * 4501 Hatfield Street, Pittsburgh, PA 15201 + * http://www.carnegierobotics.com + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Carnegie Robotics, LLC nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CARNEGIE ROBOTICS, LLC BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Significant history (date, user, job code, action): + * 2024-09-19, patrick.smith@carnegierobotics.com, IRAD, Copied from CamGetConfigMessage.hh. + **/ + +#ifndef LibMultiSense_SecondaryAppRegisteredApps +#define LibMultiSense_SecondaryAppRegisteredApps + +#include "MultiSense/details/utility/Portability.hh" + +namespace crl { +namespace multisense { +namespace details { +namespace wire { + +class SecondaryAppRegisteredApp { +public: + static CRL_CONSTEXPR VersionType VERSION = 1; + + VersionType appVersion; + std::string appName; + + template + void serialize(Archive& message, + const VersionType version) + { + (void) version; + message & appVersion; + message & appName; + } +}; + +class SecondaryAppRegisteredApps { +public: + static CRL_CONSTEXPR VersionType VERSION = 1; + static CRL_CONSTEXPR IdType ID = ID_DATA_SECONDARY_APP_REGISTERED_APPS; + + // + // Constructors + + SecondaryAppRegisteredApps(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);}; + SecondaryAppRegisteredApps() {}; + + + std::vector apps; + + // + // Serialization routine. + + template + void serialize(Archive& message, + const VersionType version) + { + (void) version; + message & apps; + } +}; + +}}}} // namespaces + +#endif diff --git a/source/Utilities/CMakeLists.txt b/source/Utilities/CMakeLists.txt index 3a55f50..4fbedbb 100644 --- a/source/Utilities/CMakeLists.txt +++ b/source/Utilities/CMakeLists.txt @@ -74,4 +74,5 @@ add_subdirectory(LidarCalUtility) add_subdirectory(PointCloudUtility) add_subdirectory(RectifiedFocalLengthUtility) add_subdirectory(SaveImageUtility) +add_subdirectory(SecondaryAppTestUtility) add_subdirectory(VersionInfoUtility) diff --git a/source/Utilities/FeatureDetectorUtility/CMakeLists.txt b/source/Utilities/FeatureDetectorUtility/CMakeLists.txt index 29b1118..be3990c 100644 --- a/source/Utilities/FeatureDetectorUtility/CMakeLists.txt +++ b/source/Utilities/FeatureDetectorUtility/CMakeLists.txt @@ -18,14 +18,15 @@ if ((CMAKE_CXX_COMPILER_ID MATCHES MSVC AND # Setup the executable that we will use. # - add_executable(FeatureDetectorUtility FeatureDetectorUtility.cc) + add_executable(FeatureDetectorUtility FeatureDetectorUtility.cc FeatureDetectorUtilities.cc) # # Specify libraries against which to link. # target_link_libraries (FeatureDetectorUtility ${MULTISENSE_UTILITY_LIBS}) - + target_include_directories (FeatureDetectorUtility PRIVATE include) + target_include_directories (FeatureDetectorUtility PRIVATE ../../source/LibMultiSense/details/include) # # Specify the install location # diff --git a/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtilities.cc b/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtilities.cc new file mode 100644 index 0000000..df87082 --- /dev/null +++ b/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtilities.cc @@ -0,0 +1,83 @@ +/** + * @file LibMultiSense/FeatureDetectorMeta.hh + * + * Copyright 2013-2024 + * Carnegie Robotics, LLC + * 4501 Hatfield Street, Pittsburgh, PA 15201 + * http://www.carnegierobotics.com + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Carnegie Robotics, LLC nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CARNEGIE ROBOTICS, LLC BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Significant history (date, user, job code, action): + * 2024-22-11, patrick.smith@carnegierobotics.com, IRAD, Moved file. + **/ + + +#include "FeatureDetectorUtilities.hh" + +namespace feature_detector +{ + +Status secondaryAppDataExtract(feature_detector::Header &header, const secondary_app::Header &orig) +{ + + crl::multisense::details::utility::BufferStreamReader stream(reinterpret_cast(orig.secondaryAppDataP), orig.secondaryAppDataLength); + wire::FeatureDetector featureDetector(stream, 1); + + crl::multisense::details::utility::BufferStreamReader metaStream(reinterpret_cast(orig.secondaryAppMetadataP), orig.secondaryAppMetadataLength); + wire::FeatureDetectorMeta _meta(metaStream, 1); + + header.source = featureDetector.source; + 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; + + const size_t startDescriptor=featureDetector.numFeatures*sizeof(wire::Feature); + + uint8_t * dataP = reinterpret_cast(featureDetector.dataP); + for (size_t i = 0; i < featureDetector.numFeatures; i++) { + feature_detector::Feature f = *reinterpret_cast(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(dataP + (startDescriptor + (j * sizeof(wire::Descriptor)))); + header.descriptors.push_back(d); + } + + return Status_Ok; +} + +} diff --git a/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc b/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc index d7ede8f..5e25fbb 100644 --- a/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc +++ b/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc @@ -65,6 +65,8 @@ #include #include +#include "FeatureDetectorUtilities.hh" + using namespace crl::multisense; namespace { // anonymous @@ -345,43 +347,53 @@ 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(userDataP); + void * buffer = userData->channelP->reserveCallbackBuffer(); + Status s = feature_detector::secondaryAppDataExtract(fHeader, header); - if ((header.source == Source_Feature_Left) - || (header.source == Source_Feature_Rectified_Left)) { + if (s != Status_Ok) + { + fprintf(stderr, "%s Error Secondary App Data extraction failed\n", __func__ ); + userData->channelP->releaseCallbackBuffer(buffer); + return; + } - auto it = userData->elapsedTime.find(header.frameId); + if ((fHeader.source == feature_detector::Source_Feature_Left) + || (fHeader.source == feature_detector::Source_Feature_Rectified_Left)) { + + 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(header.frameId, t)); + userData->elapsedTime.insert(std::pair(fHeader.frameId, t)); } else { it->second.featureTime = std::chrono::system_clock::now(); - std::cout << "Feature received after image " << header.frameId - << " Delta: " - << std::chrono::duration_cast(it->second.featureTime - it->second.imageTime).count() - << "ms\n"; + std::cout << "Feature received after image " << fHeader.frameId + << " Delta: " + << std::chrono::duration_cast(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"; + userData->channelP->releaseCallbackBuffer(buffer); } } // anonymous @@ -496,9 +508,40 @@ int main(int argc, } { - system::FeatureDetectorConfig fcfg; - status = channelP->getFeatureDetectorConfig(fcfg); + 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() ); + + feature_detector::FeatureDetectorConfig c; + c.setNumberOfFeatures(5000); + c.setGrouping(1); + c.setMotion(0); + status = channelP->setSecondaryAppConfig(c); + if (Status_Ok != status) + { + std::cerr << "Error failed to set featureDetectorConfig apps\n"; + return -2; + } + + std::cout << "Successfully Configured Feature Detector\n"; + + feature_detector::FeatureDetectorConfig fcfg; + + status = channelP->getSecondaryAppConfig(fcfg); if (Status_Ok != status) { std::cerr << "Failed to get feature detector config: " << Channel::statusString(status) << std::endl; goto clean_out; @@ -516,7 +559,7 @@ int main(int argc, fcfg.setGrouping(true); fcfg.setMotion(1); - status = channelP->setFeatureDetectorConfig(fcfg); + status = channelP->setSecondaryAppConfig(fcfg); if (Status_Ok != status) { std::cerr << "Failed to set feature detector config\n"; } @@ -577,7 +620,7 @@ int main(int argc, status = channelP->startStreams((operatingMode.supportedDataSources & Source_Luma_Left) | (operatingMode.supportedDataSources & Source_Luma_Right) | - Source_Feature_Left|Source_Feature_Right); + feature_detector::Source_Feature_Left|feature_detector::Source_Feature_Right); if (Status_Ok != status) { std::cerr << "Failed to start streams: " << Channel::statusString(status) << std::endl; goto clean_out; @@ -588,18 +631,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); } diff --git a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorConfigMessage.hh b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorConfig.hh similarity index 68% rename from source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorConfigMessage.hh rename to source/Utilities/FeatureDetectorUtility/include/FeatureDetectorConfig.hh index ebcd626..198de67 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorConfigMessage.hh +++ b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorConfig.hh @@ -1,12 +1,10 @@ /** - * @file LibMultiSense/FeatureDetectorConfigMessage.hh - * - * This message contains the current feature detector configuration. + * @file LibMultiSense/FeatureDetectorMessage.hh * * Copyright 2013-2024 * Carnegie Robotics, LLC * 4501 Hatfield Street, Pittsburgh, PA 15201 - * http://www.carnegiearobotics.com + * http://www.carnegierobotics.com * * All rights reserved. * @@ -33,27 +31,29 @@ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * * Significant history (date, user, job code, action): - * 2024-25-01, patrick.smith@carnegierobotics.com, IRAD, Created file. + * 2024-22-11, patrick.smith@carnegierobotics.com, IRAD, Created file. **/ -#ifndef LibMultisense_FeatureDetectorConfigMessage -#define LibMultisense_FeatureDetectorConfigMessage + +#ifndef __FEATURE_DETECTOR_CONFIG_H__ +#define __FEATURE_DETECTOR_CONFIG_H__ #include "MultiSense/details/utility/Portability.hh" -#include "MultiSense/details/wire/Protocol.hh" +#include "MultiSense/MultiSenseTypes.hh" namespace crl { namespace multisense { namespace details { namespace wire { -class FeatureDetectorConfig { -public: - static CRL_CONSTEXPR IdType ID = ID_DATA_FEATURE_DETECTOR_CONFIG; - static CRL_CONSTEXPR VersionType VERSION = 1; +struct FeatureDetectorConfigParams { + + + static CRL_CONSTEXPR VersionType VERSION = 1; // - // Parameters representing the current camera configuration + // The message version + VersionType version; // // The maximum number of features detected per image @@ -70,28 +70,20 @@ public: // Current Octave: 3 uint32_t motion; - // - // Constructors - - FeatureDetectorConfig(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);}; - FeatureDetectorConfig() {}; - - // - // Serialization routine - - template - void serialize(Archive& message, - const VersionType version) + FeatureDetectorConfigParams ( ) : + version(VERSION), + numberOfFeatures(1500), + grouping(true), + motion(1) { - (void) version; - - message & numberOfFeatures; - message & grouping; - message & motion; } + }; -}}}} // namespaces +} +} +} +} -#endif +#endif /* end of include guard: __FEATURE_DETECTOR_CONFIG_H__ */ diff --git a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorMessage.hh b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorMessage.hh similarity index 69% rename from source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorMessage.hh rename to source/Utilities/FeatureDetectorUtility/include/FeatureDetectorMessage.hh index dbf1286..e0167b0 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorMessage.hh +++ b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorMessage.hh @@ -34,6 +34,7 @@ * * Significant history (date, user, job code, action): * 2024-25-01, patrick.smith@carnegierobotics.com, IRAD, created file. + * 2024-22-11, patrick.smith@carnegierobotics.com, IRAD, Moved file. **/ #ifndef LibMultiSense_FeatureDetectorMessage @@ -42,13 +43,14 @@ #include #include "MultiSense/details/utility/Portability.hh" +#include "MultiSense/details/wire/Protocol.hh" namespace crl { namespace multisense { namespace details { namespace wire { -class Feature { +class __attribute__((__packed__)) Feature { public: uint16_t x; @@ -58,68 +60,30 @@ public: uint8_t octave; uint8_t descriptor; -#ifndef SENSORPOD_FIRMWARE - template - void serialize(Archive& message, - const VersionType version) - { - (void) version; - message & x; - message & y; - message & angle; - message & resp; - message & octave; - message & descriptor; - } -#endif // !SENSORPOD_FIRMWARE }; -class Descriptor { +class __attribute__((__packed__)) Descriptor { public: uint32_t d[8]; - -#ifndef SENSORPOD_FIRMWARE - template - void serialize(Archive& message, - const VersionType version) - { - (void) version; - for (size_t i = 0; i < 8; i++) - { - message & d[i]; - } - } -#endif // !SENSORPOD_FIRMWARE }; class WIRE_HEADER_ATTRIBS_ FeatureDetectorHeader { public: - static CRL_CONSTEXPR IdType ID = ID_DATA_FEATURE_DETECTOR; - static CRL_CONSTEXPR VersionType VERSION = 2; - -#ifdef SENSORPOD_FIRMWARE - IdType id; - VersionType version; -#endif // SENSORPOD_FIRMWARE - uint32_t source; + static CRL_CONSTEXPR VersionType VERSION = 1; + wire::VersionType version; + uint64_t source; int64_t frameId; uint16_t numFeatures; uint16_t numDescriptors; - uint32_t sourceExtended; - FeatureDetectorHeader() : -#ifdef SENSORPOD_FIRMWARE - id(ID), version(VERSION), -#endif // SENSORPOD_FIRMWARE source(0), frameId(0), numFeatures(0), - numDescriptors(0), - sourceExtended(0) - {}; + numDescriptors(0) + {}; }; @@ -127,11 +91,7 @@ public: class FeatureDetector : public FeatureDetectorHeader { public: - static CRL_CONSTEXPR IdType ID = ID_DATA_FEATURE_DETECTOR; static CRL_CONSTEXPR VersionType VERSION = 1; - static CRL_CONSTEXPR uint16_t FEATURE_TYPE_IMAGE_LEFT = 1; - static CRL_CONSTEXPR uint16_t FEATURE_TYPE_IMAGE_RIGHT = 2; - void * dataP; // @@ -143,10 +103,11 @@ public: template void serialize(Archive& message, - const VersionType version) + const VersionType _version) { - (void) version; + (void) _version; + message & version; message & source; message & frameId; message & numFeatures; @@ -164,20 +125,13 @@ public: message.seek(message.tell() + featureDataSize); } - if (version >= 2) - { - message & sourceExtended; - } - else - { - sourceExtended = 0; - } } }; #endif // !SENSORPOD_FIRMWARE + }}}} // namespaces #endif diff --git a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorMetaMessage.hh b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorMetaMessage.hh similarity index 80% rename from source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorMetaMessage.hh rename to source/Utilities/FeatureDetectorUtility/include/FeatureDetectorMetaMessage.hh index da7e006..77e5551 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorMetaMessage.hh +++ b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorMetaMessage.hh @@ -32,6 +32,7 @@ * * Significant history (date, user, job code, action): * 2024-01-25, patrick.smith@carnegierobotics.com, IRAD, created file. + * 2024-22-11, patrick.smith@carnegierobotics.com, IRAD, Moved file. **/ #ifndef LibMultiSense_FeatureDetectorMetadataMessage @@ -40,20 +41,17 @@ #include #include "MultiSense/details/utility/Portability.hh" +#include "MultiSense/details/wire/Protocol.hh" namespace crl { namespace multisense { namespace details { namespace wire { - class WIRE_HEADER_ATTRIBS_ FeatureDetectorMetaHeader { +class WIRE_HEADER_ATTRIBS_ FeatureDetectorMetaHeader { public: - static CRL_CONSTEXPR IdType ID = ID_DATA_FEATURE_DETECTOR_META; static CRL_CONSTEXPR VersionType VERSION = 1; -#ifdef SENSORPOD_FIRMWARE - IdType id; - VersionType version; -#endif // SENSORPOD_FIRMWARE + wire::VersionType version; uint32_t length; uint32_t source; int64_t frameId; @@ -70,29 +68,26 @@ namespace wire { uint16_t numFeatures; uint16_t numDescriptors; - FeatureDetectorMetaHeader() : - #ifdef SENSORPOD_FIRMWARE - id(ID), - version(VERSION), - #endif // SENSORPOD_FIRMWARE - length(0), - source(0), - frameId(0), - timeSeconds(0), - timeNanoSeconds(0), - ptpNanoSeconds(0), - octaveWidth(0), - octaveHeight(0), - numOctaves(0), - scaleFactor(0), - motionStatus(0), - averageXMotion(0), - averageYMotion(0), - numFeatures(0), - numDescriptors(0) - {}; - - }; + FeatureDetectorMetaHeader() : + version(VERSION), + length(0), + source(0), + frameId(0), + timeSeconds(0), + timeNanoSeconds(0), + ptpNanoSeconds(0), + octaveWidth(0), + octaveHeight(0), + numOctaves(0), + scaleFactor(0), + motionStatus(0), + averageXMotion(0), + averageYMotion(0), + numFeatures(0), + numDescriptors(0) + {}; + +}; #ifndef SENSORPOD_FIRMWARE @@ -110,9 +105,10 @@ public: template void serialize(Archive& message, - const VersionType version) + const VersionType _version) { - (void) version; + (void) _version; + message & version; message & length; message & source; message & frameId; diff --git a/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorUtilities.hh b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorUtilities.hh new file mode 100644 index 0000000..6c56fe7 --- /dev/null +++ b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorUtilities.hh @@ -0,0 +1,208 @@ +/** + * @file LibMultiSense/FeatureDetectorMeta.hh + * + * Copyright 2013-2024 + * Carnegie Robotics, LLC + * 4501 Hatfield Street, Pittsburgh, PA 15201 + * http://www.carnegierobotics.com + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Carnegie Robotics, LLC nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CARNEGIE ROBOTICS, LLC BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Significant history (date, user, job code, action): + * 2024-22-11, patrick.smith@carnegierobotics.com, IRAD, Created file. + **/ + +#ifndef __FEATURE_DETECTOR_UTILITIES_H__ +#define __FEATURE_DETECTOR_UTILITIES_H__ + +#include "MultiSense/details/utility/Portability.hh" +#include "MultiSense/details/utility/Thread.hh" +#include "MultiSense/details/utility/BufferStream.hh" +#include "MultiSense/details/utility/Units.hh" +#include "MultiSense/details/listeners.hh" + +#include "FeatureDetectorMessage.hh" +#include "FeatureDetectorMetaMessage.hh" +#include "FeatureDetectorConfig.hh" + +using namespace crl::multisense; +using namespace crl::multisense::details; + +namespace feature_detector +{ + +static CRL_CONSTEXPR DataSource Source_Feature_Left = Source_Secondary_App_Data_0; +static CRL_CONSTEXPR DataSource Source_Feature_Right = Source_Secondary_App_Data_1; +static CRL_CONSTEXPR DataSource Source_Feature_Aux = Source_Secondary_App_Data_2; +static CRL_CONSTEXPR DataSource Source_Feature_Rectified_Left = Source_Secondary_App_Data_3; +static CRL_CONSTEXPR DataSource Source_Feature_Rectified_Right = Source_Secondary_App_Data_4; +static CRL_CONSTEXPR DataSource Source_Feature_Rectified_Aux = Source_Secondary_App_Data_5; + +/** The recommended maximum number of features for full resolution camera operation */ +static CRL_CONSTEXPR int RECOMMENDED_MAX_FEATURES_FULL_RES = 5000; +/** The recommended maximum number of features for quarter resolution camera operation */ +static CRL_CONSTEXPR int RECOMMENDED_MAX_FEATURES_QUARTER_RES = 1500; + +class MULTISENSE_API FeatureDetectorConfig: public crl::multisense::system::SecondaryAppConfig { + + private: + + wire::FeatureDetectorConfigParams mConfigItems; + + public: + /** + * Query the maximum number of features applied to the camera feature detector + * + * @return Return the current maximum number of features + */ + uint32_t numberOfFeatures() const { return mConfigItems.numberOfFeatures; }; + + /** + * Query the status of the feature detector feature grouping + * + * @return Return the current feature grouping status + */ + bool grouping() const { return mConfigItems.grouping; }; + + /** + * Query the status of the feature detector motion detection + * + * @return Return the current feature detector motion detection status + */ + bool motion() const { return mConfigItems.motion; }; + + /** + * Set the maximum number of features applied to the camera feature detector. + * Current recommended settings. + * Full Resolution: 5000 Features @5FPS + * Quarter Resolution: 1500 Features @15FPS + * + * @param numberOfFeatures The maximum number of features. + */ + + void setNumberOfFeatures(const uint32_t &numberOfFeatures) { + + if (numberOfFeatures > RECOMMENDED_MAX_FEATURES_FULL_RES) + { + std::cout << "WARNING: The number of features requested is above recommended level!" << '\n'; + std::cout << "If a performance impact is noticed reduce number of features and/or framerate of camera" << '\n'; + std::cout << "The recommended maximum camera settings when using the feature detector is:" << '\n'; + std::cout << "Quarter Res: 15FPS and 1500 Features" << '\n'; + std::cout << "Full Res: 5FPS and 5000 Features" << '\n'; + } + + mConfigItems.numberOfFeatures = numberOfFeatures; + + }; + + /** + * Set the feature grouping capability the feature detector + * + * @param g The feature grouping to apply to this camera + */ + void setGrouping(const bool &g) { + mConfigItems.grouping = g; + } + + /** + * Set the feature motion detection capability of the feature detector + * Functions to enable motion detection on Octave 3 + * + * + * @param m The feature detector motion detector. + */ + void setMotion(const uint32_t &m) { + mConfigItems.motion = m; + } + + /** Default constructor */ + FeatureDetectorConfig() + { + mConfigItems.numberOfFeatures = RECOMMENDED_MAX_FEATURES_QUARTER_RES; + mConfigItems.grouping = true; + mConfigItems.motion = 1; + }; + + void serialize( void ) + { + memcpy(data, &mConfigItems, sizeof(mConfigItems)); + dataLength = sizeof(mConfigItems); + } + +}; + +struct Feature { + uint16_t x; + uint16_t y; + uint8_t angle; + uint8_t resp; + uint8_t octave; + uint8_t descriptor; +}; + +struct Descriptor { + uint32_t d[8]; //Descriptor is 32 bytes +}; + +class MULTISENSE_API Header : public HeaderBase { +public: + + DataSource source; + int64_t frameId; + uint32_t timeSeconds; + uint32_t timeNanoSeconds; + int64_t ptpNanoSeconds; + uint16_t octaveWidth; + uint16_t octaveHeight; + uint16_t numOctaves; + uint16_t scaleFactor; + uint16_t motionStatus; + uint16_t averageXMotion; + uint16_t averageYMotion; + uint16_t numFeatures; + uint16_t numDescriptors; + std::vector features; + std::vector descriptors; +}; + + +/** + * 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 + */ +Status secondaryAppDataExtract(feature_detector::Header &header, const secondary_app::Header &orig); + + +} // namespace util + + +#endif /* end of include guard: __FEATURE_DETECTOR_UTILITIES_H__ */ diff --git a/source/Utilities/SecondaryAppTestUtility/CMakeLists.txt b/source/Utilities/SecondaryAppTestUtility/CMakeLists.txt new file mode 100644 index 0000000..0b40777 --- /dev/null +++ b/source/Utilities/SecondaryAppTestUtility/CMakeLists.txt @@ -0,0 +1,40 @@ +# +# SecondaryAppTestUtility - Makefile +# + +# +# This utility requires c++14 +# + +if ((CMAKE_CXX_COMPILER_ID MATCHES MSVC AND + CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 14.0) OR + (CMAKE_CXX_COMPILER_ID MATCHES GNU AND + CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 4.9) OR + (CMAKE_CXX_COMPILER_ID MATCHES Clang AND + CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 3.4)) + + set(CMAKE_CXX_STANDARD 14) + + # + # Setup the executable that we will use. + # + + # + # Setup the executable that we will use. + # + + add_executable(SecondaryAppTestUtility SecondaryAppTestUtility.cc) + + # + # Specify libraries against which to link. + # + + target_link_libraries (SecondaryAppTestUtility ${MULTISENSE_UTILITY_LIBS}) + + # + # Specify the install location + # + + install(TARGETS SecondaryAppTestUtility RUNTIME DESTINATION "bin") + +endif() diff --git a/source/Utilities/SecondaryAppTestUtility/SecondaryAppTestUtility.cc b/source/Utilities/SecondaryAppTestUtility/SecondaryAppTestUtility.cc new file mode 100644 index 0000000..9fa70c8 --- /dev/null +++ b/source/Utilities/SecondaryAppTestUtility/SecondaryAppTestUtility.cc @@ -0,0 +1,218 @@ +/** + * @file SecondaryAppTestUtility/SecondaryAppTestUtility.cc + * + * Copyright 2013-2023 + * Carnegie Robotics, LLC + * 4501 Hatfield Street, Pittsburgh, PA 15201 + * http://www.carnegierobotics.com + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Carnegie Robotics, LLC nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CARNEGIE ROBOTICS, LLC BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Significant history (date, user, job code, action): + * 2023-09-19, patrick.smith@carnegierobotics.com, IRAD, Created file. + **/ + +#ifdef WIN32 +#ifndef WIN32_LEAN_AND_MEAN +#define WIN32_LEAN_AND_MEAN 1 +#endif + +#include +#include +#else +#include +#endif + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +using namespace crl::multisense; + +namespace { // anonymous + +volatile bool doneG = false; + +void usage(const char *programNameP) +{ + std::cerr << "USAGE: " << programNameP << " []" << std::endl; + std::cerr << "Where are:" << std::endl; + std::cerr << "\t-a : IPV4 address (default=10.66.171.21)" << std::endl; + std::cerr << "\t-m : default=7200" << std::endl; + std::cerr << "\t-f : FILE to log IMU data (stdout by default)" << std::endl; + + exit(1); +} + +#ifdef WIN32 +BOOL WINAPI signalHandler(DWORD dwCtrlType) +{ + CRL_UNUSED (dwCtrlType); + std::cerr << "Shutting down on signal: CTRL-C" << std::endl; + doneG = true; + return TRUE; +} +#else +void signalHandler(int sig) +{ + std::cerr << "Shutting down on signal: " << strsignal(sig) << std::endl; + doneG = true; +} +#endif + +void secondaryAppCallback(const secondary_app::Header& header, void* userDataP) +{ + (void) userDataP; + + std::cout << "----------------------------" << std::endl; + std::cout << "frameId: " << header.frameId << std::endl; +} + +} // anonymous + +int main(int argc, + char **argvPP) +{ + std::string currentAddress = "10.66.171.21"; + uint32_t mtu = 7200; + DataSource streams = Source_Luma_Left; + streams |= Source_Secondary_App_Data_0; + image::Config cfg; + +#if WIN32 + SetConsoleCtrlHandler (signalHandler, TRUE); +#else + signal(SIGINT, signalHandler); +#endif + + // + // Parse args + + int c; + + while(-1 != (c = getopt(argc, argvPP, "a:m:v"))) + switch(c) { + case 'a': currentAddress = std::string(optarg); break; + case 'm': mtu = atoi(optarg); break; + default: usage(*argvPP); break; + } + + // + // Initialize communications. + + Channel *channelP = Channel::Create(currentAddress); + if (NULL == channelP) { + std::cerr << "Failed to establish communications with \"" << currentAddress << "\"" << std::endl; + return -1; + } + + // + // Query firmware version + + system::VersionInfo v; + + Status status = channelP->getVersionInfo(v); + if (Status_Ok != status) { + std::cerr << "Failed to query sensor version: " << Channel::statusString(status) << std::endl; + goto clean_out; + } + + // + // Turn off all streams by default + + status = channelP->stopStreams(Source_All); + if (Status_Ok != status) { + std::cerr << "Failed to stop streams: " << Channel::statusString(status) << std::endl; + goto clean_out; + } + + // + // Change MTU + + status = channelP->setMtu(mtu); + if (Status_Ok != status) { + std::cerr << "Failed to set MTU to " << mtu << ": " << Channel::statusString(status) << std::endl; + goto clean_out; + } + + // + // Use standard controls to configure the camera. + + cfg.setResolution(1920,1200); + cfg.setFps(5.0); + + status = channelP->getImageConfig(cfg); + if (Status_Ok != status) { + std::cerr << "Reconfigure: failed to query image config: " << Channel::statusString(status) << std::endl; + goto clean_out; + } + + + + // + // Add callbacks + + channelP->addIsolatedCallback(secondaryAppCallback); + + + // + // Start streaming + + status = channelP->startStreams(streams); + if (Status_Ok != status) { + std::cerr << "Failed to start streams: " << Channel::statusString(status) << std::endl; + goto clean_out; + } + + while(!doneG) + usleep(100000); + + // + // Stop streaming + + status = channelP->stopStreams(Source_All); + if (Status_Ok != status) { + std::cerr << "Failed to stop streams: " << Channel::statusString(status) << std::endl; + } + + // + // Report simple stats + + +clean_out: + + Channel::Destroy(channelP); + return 0; +}