From 6eb104ed264a8bc147d4c533d6806fb8486867ab Mon Sep 17 00:00:00 2001 From: psmithcrl Date: Tue, 19 Sep 2023 15:02:37 -0400 Subject: [PATCH 01/22] Added secondary application compiles, untested --- source/LibMultiSense/details/channel.cc | 2 + source/LibMultiSense/details/dispatch.cc | 31 +++ source/LibMultiSense/details/public.cc | 87 +++++++ .../include/MultiSense/MultiSenseChannel.hh | 37 +++ .../include/MultiSense/MultiSenseTypes.hh | 61 +++++ .../include/MultiSense/details/channel.hh | 12 +- .../include/MultiSense/details/listeners.hh | 1 + .../MultiSense/details/wire/Protocol.hh | 5 + .../details/wire/SecondaryAppConfigMessage.hh | 82 +++++++ .../wire/SecondaryAppControlMessage.hh | 83 +++++++ .../wire/SecondaryAppGetConfigMessage.hh | 75 ++++++ .../details/wire/SecondaryAppMessage.hh | 129 +++++++++++ source/Utilities/CMakeLists.txt | 1 + .../SecondaryAppTestUtility/CMakeLists.txt | 40 ++++ .../SecondaryAppTestUtility.cc | 213 ++++++++++++++++++ 15 files changed, 858 insertions(+), 1 deletion(-) create mode 100644 source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppConfigMessage.hh create mode 100644 source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppControlMessage.hh create mode 100644 source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppGetConfigMessage.hh create mode 100644 source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMessage.hh create mode 100644 source/Utilities/SecondaryAppTestUtility/CMakeLists.txt create mode 100644 source/Utilities/SecondaryAppTestUtility/SecondaryAppTestUtility.cc diff --git a/source/LibMultiSense/details/channel.cc b/source/LibMultiSense/details/channel.cc index 4228bfa6..0febdf40 100644 --- a/source/LibMultiSense/details/channel.cc +++ b/source/LibMultiSense/details/channel.cc @@ -470,6 +470,7 @@ 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) wire_mask |= wire::SOURCE_SECONDARY_APP_DATA; return wire_mask; } @@ -509,6 +510,7 @@ 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) api_mask |= Source_Secondary_App_Data; return api_mask; } diff --git a/source/LibMultiSense/details/dispatch.cc b/source/LibMultiSense/details/dispatch.cc index 3bc519e8..747958e2 100644 --- a/source/LibMultiSense/details/dispatch.cc +++ b/source/LibMultiSense/details/dispatch.cc @@ -81,6 +81,9 @@ #include "MultiSense/details/wire/GroundSurfaceModel.hh" #include "MultiSense/details/wire/ApriltagDetections.hh" +#include "MultiSense/details/wire/SecondaryAppMessage.hh" +#include "MultiSense/details/wire/SecondaryAppControlMessage.hh" +#include "MultiSense/details/wire/SecondaryAppConfigMessage.hh" #include @@ -210,6 +213,21 @@ void impl::dispatchAprilTagDetections(apriltag::Header& header) (*it)->dispatch(header); } +// +// Publish Secondary App Data + +void impl::dispatchSecondaryApplication(secondary_app::Header& header) +{ + utility::ScopedLock lock(m_dispatchLock); + + std::list::const_iterator it; + + for(it = m_secondaryAppListeners.begin(); + it != m_secondaryAppListeners.end(); + it ++) + (*it)->dispatch(header); +} + // // Dispatch incoming messages @@ -542,6 +560,16 @@ void impl::dispatch(utility::BufferStreamWriter& buffer) dispatchAprilTagDetections(header); break; } + case MSG_ID(wire::SecondaryAppData::ID): + { + wire::SecondaryAppData SecondaryApp(stream, version); + + secondary_app::Header header; + + //TODO: add header stuff + dispatchSecondaryApplication(header); + break; + } case MSG_ID(wire::Ack::ID): break; // handle below case MSG_ID(wire::CamConfig::ID): @@ -610,6 +638,9 @@ 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; 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 9b9951d0..0de32548 100644 --- a/source/LibMultiSense/details/public.cc +++ b/source/LibMultiSense/details/public.cc @@ -105,6 +105,11 @@ #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/SecondaryAppMessage.hh" + namespace crl { namespace multisense { namespace details { @@ -298,6 +303,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 @@ -466,6 +492,34 @@ Status impl::removeIsolatedCallback(apriltag::Callback callback) return Status_Error; } +// +// Removes a secondary app listener + +Status impl::removeIsolatedCallback(secondary_app::Callback callback) +{ + try { + utility::ScopedLock lock(m_dispatchLock); + + std::list::iterator it; + for(it = m_secondaryAppListeners.begin(); + it != m_secondaryAppListeners.end(); + it ++) { + + if ((*it)->callback() == callback) { + delete *it; + m_secondaryAppListeners.erase(it); + return Status_Ok; + } + } + + } catch (const std::exception& e) { + CRL_DEBUG("exception: %s\n", e.what()); + return Status_Exception; + } + + return Status_Error; +} + // // Reserve the current callback buffer being used in a dispatch thread @@ -1434,6 +1488,39 @@ Status impl::setApriltagParams (const system::ApriltagParams& params) return waitAck(w); } + +// +// 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; + + config.framesPerSecond = d.framesPerSecond; + + return Status_Ok; +} + + +// +// Set camera configuration +// +// Currently several sensor messages are combined and presented +// to the user as one. + +Status impl::setSecondaryAppConfig(const system::SecondaryAppConfig& c) +{ + wire::SecondaryAppControl cmd; + cmd.framesPerSecond = c.framesPerSecond; + return waitAck(cmd); +} + + // // Sets the device info diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh b/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh index fd2e2b03..4ed9d7eb 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh @@ -350,6 +350,31 @@ public: */ virtual Status addIsolatedCallback(apriltag::Callback callback, void *userDataP=NULL) = 0; + /** + * Add a user defined callback attached to the Secondary result stream. + * + * Each callback will create a unique internal thread + * dedicated to servicing the callback. + * + * 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. + * + * @return A crl::multisense::Status indicating if the callback registration + * succeeded or failed + */ + virtual Status addIsolatedCallback(secondary_app::Callback callback, + void *userDataP=NULL) = 0; /** * Unregister a user defined image::Callback. This stops the callback @@ -435,6 +460,18 @@ public: virtual Status removeIsolatedCallback(apriltag::Callback callback) = 0; + /** + * Unregister a user defined secondary_app::Callback. This stops the callback + * from receiving ground surface data + * + * @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(secondary_app::Callback callback) = 0; + /** * Reserve image or lidar data within a isolated callback so it is available * after the callback returns. diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh index 776c2ec1..f935fafa 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh @@ -147,6 +147,7 @@ static CRL_CONSTEXPR DataSource Source_Compressed_Aux = (1U<<14); static CRL_CONSTEXPR DataSource Source_Compressed_Rectified_Left = (1U<<15); static CRL_CONSTEXPR DataSource Source_Compressed_Rectified_Right = (1U<<16); static CRL_CONSTEXPR DataSource Source_Compressed_Rectified_Aux = (1U<<17); +static CRL_CONSTEXPR DataSource Source_Secondary_App_Data = (1U<<18); /** * Use Roi_Full_Image as the height and width when setting the autoExposureRoi @@ -2976,6 +2977,57 @@ typedef void (*Callback)(const Header& header, } // namespace apriltag +namespace secondary_app { + +/** + * 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; + /** Bits per pixel in the secondaryAppData */ + uint32_t bitsPerPixel; + /** Width of the secondaryAppData */ + uint32_t width; + /** Height of the secondaryAppData*/ + uint32_t height; + /** 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; + + /** + * Default Constructor + */ + Header() + : source(Source_Unknown) {}; + + /** + * Member function used to determine if the data contained in the header + * is contained in a specific image mask + */ + virtual bool inMask(DataSource mask) { return (mask & source) != 0;}; + +}; + /** + * Function pointer for receiving callbacks for apriltag data + */ + typedef void (*Callback)(const Header& header, + void *userDataP); +} // namespace secondary_app + namespace system { /** @@ -3850,6 +3902,15 @@ class MULTISENSE_API ApriltagParams { class MULTISENSE_API PtpStatus { }; +class MULTISENSE_API SecondaryAppConfig { +public: + float framesPerSecond; + + SecondaryAppConfig(): + framesPerSecond(5.0f) + {}; +}; + } // namespace system } // namespace multisense } // namespace crl diff --git a/source/LibMultiSense/include/MultiSense/details/channel.hh b/source/LibMultiSense/include/MultiSense/details/channel.hh index ad876e23..45bc42e8 100644 --- a/source/LibMultiSense/include/MultiSense/details/channel.hh +++ b/source/LibMultiSense/include/MultiSense/details/channel.hh @@ -120,6 +120,9 @@ public: virtual Status addIsolatedCallback (apriltag::Callback callback, void *userDataP); + virtual Status addIsolatedCallback (secondary_app::Callback callback, + void *userDataP); + virtual Status removeIsolatedCallback(image::Callback callback); virtual Status removeIsolatedCallback(lidar::Callback callback); virtual Status removeIsolatedCallback(pps::Callback callback); @@ -127,7 +130,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(secondary_app::Callback callback); virtual void* reserveCallbackBuffer (); virtual Status releaseCallbackBuffer (void *referenceP); @@ -215,6 +218,10 @@ public: uint32_t bufferSize); virtual Status getLocalUdpPort (uint16_t& port); + + virtual Status getSecondaryAppConfig (system::SecondaryAppConfig & c); + virtual Status setSecondaryAppConfig (const system::SecondaryAppConfig & c); + private: // @@ -320,6 +327,7 @@ private: 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_SECONDARY_APP_QUEUE_SIZE = 8; // // The maximum number of directed streams @@ -462,6 +470,7 @@ private: std::list m_compressedImageListeners; std::list m_groundSurfaceSplineListeners; std::list m_aprilTagDetectionListeners; + std::list m_secondaryAppListeners; // // A message signal interface @@ -536,6 +545,7 @@ private: compressed_image::Header& header); void dispatchGroundSurfaceSpline(ground_surface::Header& header); void dispatchAprilTagDetections(apriltag::Header& header); + void dispatchSecondaryApplication(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 fab47fe2..a8811fde 100644 --- a/source/LibMultiSense/include/MultiSense/details/listeners.hh +++ b/source/LibMultiSense/include/MultiSense/details/listeners.hh @@ -230,6 +230,7 @@ typedef Listener ImuListen typedef Listener CompressedImageListener; typedef Listener GroundSurfaceSplineListener; typedef Listener AprilTagDetectionListener; +typedef Listener SecondaryAppListener; } // namespace details } // namespace multisense diff --git a/source/LibMultiSense/include/MultiSense/details/wire/Protocol.hh b/source/LibMultiSense/include/MultiSense/details/wire/Protocol.hh index cf9cbfe2..22376783 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/Protocol.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/Protocol.hh @@ -190,6 +190,8 @@ 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_SECONDARY_APP_CONTROL = 0x002e; +static CRL_CONSTEXPR IdType ID_CMD_SECONDARY_APP_GET_CONFIG = 0x002f; // // Data @@ -228,6 +230,8 @@ static CRL_CONSTEXPR IdType ID_DATA_APRILTAG_DETECTIONS_MESSAGE = 0x0122; static CRL_CONSTEXPR IdType ID_DATA_SYS_APRILTAG_PARAM = 0x0123; static CRL_CONSTEXPR IdType ID_DATA_CAM_AUX_CONFIG = 0x0124; static CRL_CONSTEXPR IdType ID_CMD_REMOTE_HEAD_CONFIG = 0x0125; +static CRL_CONSTEXPR IdType ID_DATA_SECONDARY_APP = 0x0126; +static CRL_CONSTEXPR IdType ID_DATA_SECONDARY_APP_CONFIG = 0x0127; // // Data sources @@ -268,6 +272,7 @@ static CRL_CONSTEXPR SourceType SOURCE_COMPRESSED_AUX = (1U<<14); static CRL_CONSTEXPR SourceType SOURCE_COMPRESSED_RECTIFIED_LEFT = (1U<<15); static CRL_CONSTEXPR SourceType SOURCE_COMPRESSED_RECTIFIED_RIGHT = (1U<<16); // same as SOURCE_JPEG_LEFT static CRL_CONSTEXPR SourceType SOURCE_COMPRESSED_RECTIFIED_AUX = (1U<<17); // same as SOURCE_RGB_LEFT +static CRL_CONSTEXPR SourceType SOURCE_SECONDARY_APP_DATA = (1U<<18); // same as SOURCE_RGB_LEFT static CRL_CONSTEXPR SourceType SOURCE_IMAGES = (SOURCE_RAW_LEFT | SOURCE_RAW_RIGHT | diff --git a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppConfigMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppConfigMessage.hh new file mode 100644 index 00000000..122a56a0 --- /dev/null +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppConfigMessage.hh @@ -0,0 +1,82 @@ +/** + * @file LibMultiSense/SecondaryAppConfig.hh + * + * This message contains the current 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, Created file. + **/ + +#ifndef LibMultiSense_SecondaryAppConfig +#define LibMultiSense_SecondaryAppConfig + +#include "MultiSense/details/utility/Portability.hh" +#include "MultiSense/details/wire/Protocol.hh" + +namespace crl { +namespace multisense { +namespace details { +namespace wire { + +class SecondaryAppConfig { +public: + static CRL_CONSTEXPR IdType ID = ID_DATA_SECONDARY_APP_CONFIG; + static CRL_CONSTEXPR VersionType VERSION = 1; + + // + // Parameters representing the current camera configuration + float framesPerSecond; + + // + // Constructors + + SecondaryAppConfig(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);}; + SecondaryAppConfig(): + framesPerSecond(0.0f) + {}; + + // + // Serialization routine + + template + void serialize(Archive& message, + const VersionType version) + { + (void) version; + message & framesPerSecond; + + } +}; + +}}}} // namespaces + +#endif diff --git a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppControlMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppControlMessage.hh new file mode 100644 index 00000000..668918eb --- /dev/null +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppControlMessage.hh @@ -0,0 +1,83 @@ +/** + * @file LibMultiSense/SecondaryAppControl.hh + * + * This message contains the current 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, Created file. + **/ + +#ifndef LibMultiSense_SecondaryAppControl +#define LibMultiSense_SecondaryAppControl + +#include "MultiSense/details/utility/Portability.hh" +#include "MultiSense/details/wire/Protocol.hh" + +namespace crl { +namespace multisense { +namespace details { +namespace wire { + +class SecondaryAppControl { +public: + static CRL_CONSTEXPR IdType ID = ID_CMD_SECONDARY_APP_CONTROL; + static CRL_CONSTEXPR VersionType VERSION = 1; + + // + // Parameters representing the current camera configuration + float framesPerSecond; + + + // + // Constructors + + SecondaryAppControl(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);}; + SecondaryAppControl(): + framesPerSecond(0.0f) + {}; + + // + // Serialization routine + + template + void serialize(Archive& message, + const VersionType version) + { + (void) version; + message & framesPerSecond; + + } +}; + +}}}} // 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 00000000..38e32f93 --- /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/SecondaryAppMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMessage.hh new file mode 100644 index 00000000..33310a68 --- /dev/null +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMessage.hh @@ -0,0 +1,129 @@ +/** + * @file LibMultiSense/ImageMessage.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_SecondaryApp +#define LibMultiSense_SecondaryApp + +#include +#include + +#include "MultiSense/details/utility/Portability.hh" + +namespace crl { +namespace multisense { +namespace details { +namespace wire { + +class WIRE_HEADER_ATTRIBS_ SecondaryApp { +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 bitsPerPixel; + uint32_t length; + int64_t frameId; + uint32_t timeSeconds; + uint32_t timeMicroSeconds; + SecondaryApp() + : +#ifdef SENSORDPOD_FIRMWARE + id(ID), + version(VERSION), +#endif // SENSORPOD_FIRMWARE + source(0), + bitsPerPixel(0), + length(0), + frameId(0), + timeSeconds(0), + timeMicroSeconds(0) + {}; +}; + +#ifndef SENSORPOD_FIRMWARE + +class SecondaryAppData : public SecondaryApp { +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 & bitsPerPixel; + message & length; + message & frameId; + message & timeSeconds; + message & timeMicroSeconds; + + const uint32_t dataSize = static_cast (std::ceil(((double) bitsPerPixel / 8.0) * length)); + + if (typeid(Archive) == typeid(utility::BufferStreamWriter)) { + + message.write(dataP, dataSize); + + } else { + + dataP = message.peek(); + message.seek(message.tell() + length); + } + + } +}; + +#endif // !SENSORPOD_FIRMWARE + +}}}} // namespaces + +#endif diff --git a/source/Utilities/CMakeLists.txt b/source/Utilities/CMakeLists.txt index 6700422b..73aeee37 100644 --- a/source/Utilities/CMakeLists.txt +++ b/source/Utilities/CMakeLists.txt @@ -72,4 +72,5 @@ add_subdirectory(LidarCalUtility) add_subdirectory(PointCloudUtility) add_subdirectory(RectifiedFocalLengthUtility) add_subdirectory(SaveImageUtility) +add_subdirectory(SecondaryAppTestUtility) add_subdirectory(VersionInfoUtility) diff --git a/source/Utilities/SecondaryAppTestUtility/CMakeLists.txt b/source/Utilities/SecondaryAppTestUtility/CMakeLists.txt new file mode 100644 index 00000000..0b40777b --- /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 00000000..6819990c --- /dev/null +++ b/source/Utilities/SecondaryAppTestUtility/SecondaryAppTestUtility.cc @@ -0,0 +1,213 @@ +/** + * @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; + + 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; + } + + // + // Enable apriltag profile + + 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(Source_Secondary_App_Data); + 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; +} From b5d2be260fa4fb852c9339cf3c0168e83de542b9 Mon Sep 17 00:00:00 2001 From: psmithcrl Date: Thu, 21 Sep 2023 12:44:18 -0400 Subject: [PATCH 02/22] Rename... Fixed dispatch, copied wire header to public header --- source/LibMultiSense/details/dispatch.cc | 10 ++++++++-- source/LibMultiSense/details/public.cc | 2 +- .../include/MultiSense/MultiSenseTypes.hh | 6 ++---- ...ndaryAppMessage.hh => SecondaryAppDataMessage.hh} | 12 ++++++------ 4 files changed, 17 insertions(+), 13 deletions(-) rename source/LibMultiSense/include/MultiSense/details/wire/{SecondaryAppMessage.hh => SecondaryAppDataMessage.hh} (93%) diff --git a/source/LibMultiSense/details/dispatch.cc b/source/LibMultiSense/details/dispatch.cc index 747958e2..7e23aaaa 100644 --- a/source/LibMultiSense/details/dispatch.cc +++ b/source/LibMultiSense/details/dispatch.cc @@ -81,7 +81,7 @@ #include "MultiSense/details/wire/GroundSurfaceModel.hh" #include "MultiSense/details/wire/ApriltagDetections.hh" -#include "MultiSense/details/wire/SecondaryAppMessage.hh" +#include "MultiSense/details/wire/SecondaryAppDataMessage.hh" #include "MultiSense/details/wire/SecondaryAppControlMessage.hh" #include "MultiSense/details/wire/SecondaryAppConfigMessage.hh" @@ -566,7 +566,13 @@ void impl::dispatch(utility::BufferStreamWriter& buffer) secondary_app::Header header; - //TODO: add header stuff + header.frameId = SecondaryApp.frameId; + header.source = SecondaryApp.source; + header.bitsPerPixel = SecondaryApp.bitsPerPixel; + header.timeSeconds = SecondaryApp.timeSeconds; + header.timeMicroSeconds = SecondaryApp.timeMicroSeconds; + header.length = SecondaryApp.length; + dispatchSecondaryApplication(header); break; } diff --git a/source/LibMultiSense/details/public.cc b/source/LibMultiSense/details/public.cc index 0de32548..038a6227 100644 --- a/source/LibMultiSense/details/public.cc +++ b/source/LibMultiSense/details/public.cc @@ -108,7 +108,7 @@ #include "MultiSense/details/wire/SecondaryAppConfigMessage.hh" #include "MultiSense/details/wire/SecondaryAppControlMessage.hh" #include "MultiSense/details/wire/SecondaryAppGetConfigMessage.hh" -#include "MultiSense/details/wire/SecondaryAppMessage.hh" +#include "MultiSense/details/wire/SecondaryAppDataMessage.hh" namespace crl { namespace multisense { diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh index f935fafa..d40bc64c 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh @@ -2991,10 +2991,8 @@ public: DataSource source; /** Bits per pixel in the secondaryAppData */ uint32_t bitsPerPixel; - /** Width of the secondaryAppData */ - uint32_t width; - /** Height of the secondaryAppData*/ - uint32_t height; + /** length of the secondaryAppData */ + uint32_t length; /** Unique ID used to describe an secondaryAppData. FrameIds increase sequentally from the device */ int64_t frameId; /** The time seconds value corresponding to when the secondaryAppData was captured*/ diff --git a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppDataMessage.hh similarity index 93% rename from source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMessage.hh rename to source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppDataMessage.hh index 33310a68..0f3d9e00 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMessage.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppDataMessage.hh @@ -1,5 +1,5 @@ /** - * @file LibMultiSense/ImageMessage.hh + * @file LibMultiSense/SecondarryAppDataMessage.hh * * Copyright 2013-2022 * Carnegie Robotics, LLC @@ -34,8 +34,8 @@ * 2023-09-19, patrick.smith@carnegierobotics.com, IRAD, created file. **/ -#ifndef LibMultiSense_SecondaryApp -#define LibMultiSense_SecondaryApp +#ifndef LibMultiSense_SecondaryAppData +#define LibMultiSense_SecondaryAppData #include #include @@ -47,7 +47,7 @@ namespace multisense { namespace details { namespace wire { -class WIRE_HEADER_ATTRIBS_ SecondaryApp { +class WIRE_HEADER_ATTRIBS_ SecondaryAppHeader { public: static CRL_CONSTEXPR IdType ID = ID_DATA_SECONDARY_APP; @@ -64,7 +64,7 @@ static CRL_CONSTEXPR VersionType VERSION = 1; int64_t frameId; uint32_t timeSeconds; uint32_t timeMicroSeconds; - SecondaryApp() + SecondaryAppHeader() : #ifdef SENSORDPOD_FIRMWARE id(ID), @@ -81,7 +81,7 @@ static CRL_CONSTEXPR VersionType VERSION = 1; #ifndef SENSORPOD_FIRMWARE -class SecondaryAppData : public SecondaryApp { +class SecondaryAppData : public SecondaryAppHeader { public: void *dataP; From 903bdde237b9a79fc61e1ea77e6a25b5c441160f Mon Sep 17 00:00:00 2001 From: psmithcrl Date: Thu, 21 Sep 2023 16:53:20 -0400 Subject: [PATCH 03/22] Pass buffer to dispatch, remove inMask --- source/LibMultiSense/details/channel.cc | 7 +++++++ source/LibMultiSense/details/dispatch.cc | 9 +++++---- .../LibMultiSense/include/MultiSense/MultiSenseTypes.hh | 6 ------ .../LibMultiSense/include/MultiSense/details/channel.hh | 3 ++- 4 files changed, 14 insertions(+), 11 deletions(-) diff --git a/source/LibMultiSense/details/channel.cc b/source/LibMultiSense/details/channel.cc index 0febdf40..5663b3ce 100644 --- a/source/LibMultiSense/details/channel.cc +++ b/source/LibMultiSense/details/channel.cc @@ -87,6 +87,7 @@ impl::impl(const std::string& address, const RemoteHeadChannel& cameraId, const m_ppsListeners(), m_imuListeners(), m_compressedImageListeners(), + m_secondaryAppListeners(), m_watch(), m_messages(), m_streamsEnabled(0), @@ -258,6 +259,11 @@ void impl::cleanup() itc != m_compressedImageListeners.end(); itc ++) delete *itc; + 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(); @@ -273,6 +279,7 @@ void impl::cleanup() m_lidarListeners.clear(); m_ppsListeners.clear(); m_imuListeners.clear(); + m_secondaryAppListeners.clear(); m_compressedImageListeners.clear(); m_rxLargeBufferPool.clear(); m_rxSmallBufferPool.clear(); diff --git a/source/LibMultiSense/details/dispatch.cc b/source/LibMultiSense/details/dispatch.cc index 7e23aaaa..df836b9a 100644 --- a/source/LibMultiSense/details/dispatch.cc +++ b/source/LibMultiSense/details/dispatch.cc @@ -216,7 +216,8 @@ void impl::dispatchAprilTagDetections(apriltag::Header& header) // // Publish Secondary App Data -void impl::dispatchSecondaryApplication(secondary_app::Header& header) +void impl::dispatchSecondaryApplication(utility::BufferStream& buffer, + secondary_app::Header& header) { utility::ScopedLock lock(m_dispatchLock); @@ -225,7 +226,7 @@ void impl::dispatchSecondaryApplication(secondary_app::Header& header) for(it = m_secondaryAppListeners.begin(); it != m_secondaryAppListeners.end(); it ++) - (*it)->dispatch(header); + (*it)->dispatch(buffer, header); } @@ -572,8 +573,8 @@ void impl::dispatch(utility::BufferStreamWriter& buffer) header.timeSeconds = SecondaryApp.timeSeconds; header.timeMicroSeconds = SecondaryApp.timeMicroSeconds; header.length = SecondaryApp.length; - - dispatchSecondaryApplication(header); + header.secondaryAppDataP = SecondaryApp.dataP; + dispatchSecondaryApplication(buffer, header); break; } case MSG_ID(wire::Ack::ID): diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh index d40bc64c..b3132de7 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh @@ -3012,12 +3012,6 @@ public: Header() : source(Source_Unknown) {}; - /** - * Member function used to determine if the data contained in the header - * is contained in a specific image mask - */ - virtual bool inMask(DataSource mask) { return (mask & source) != 0;}; - }; /** * Function pointer for receiving callbacks for apriltag data diff --git a/source/LibMultiSense/include/MultiSense/details/channel.hh b/source/LibMultiSense/include/MultiSense/details/channel.hh index 45bc42e8..be28196a 100644 --- a/source/LibMultiSense/include/MultiSense/details/channel.hh +++ b/source/LibMultiSense/include/MultiSense/details/channel.hh @@ -545,7 +545,8 @@ private: compressed_image::Header& header); void dispatchGroundSurfaceSpline(ground_surface::Header& header); void dispatchAprilTagDetections(apriltag::Header& header); - void dispatchSecondaryApplication(secondary_app::Header& header); + void dispatchSecondaryApplication(utility::BufferStream& buffer, + secondary_app::Header& header); utility::BufferStreamWriter& findFreeBuffer (uint32_t messageLength); const int64_t& unwrapSequenceId(uint16_t id); From ae9119eba8d612d45ccc64b512a7300b889ada96 Mon Sep 17 00:00:00 2001 From: psmithcrl Date: Fri, 22 Sep 2023 11:38:11 -0400 Subject: [PATCH 04/22] added camera source, and config --- .../SecondaryAppTestUtility.cc | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/source/Utilities/SecondaryAppTestUtility/SecondaryAppTestUtility.cc b/source/Utilities/SecondaryAppTestUtility/SecondaryAppTestUtility.cc index 6819990c..87bfbb39 100644 --- a/source/Utilities/SecondaryAppTestUtility/SecondaryAppTestUtility.cc +++ b/source/Utilities/SecondaryAppTestUtility/SecondaryAppTestUtility.cc @@ -107,7 +107,8 @@ int main(int argc, { std::string currentAddress = "10.66.171.21"; uint32_t mtu = 7200; - + DataSource streams = Source_Luma_Left; + streams |= Source_Secondary_App_Data; image::Config cfg; #if WIN32 @@ -167,7 +168,10 @@ int main(int argc, } // - // Enable apriltag profile + // Use standard controls to configure the camera. + + cfg.setResolution(1920,1200); + cfg.setFps(5.0); status = channelP->getImageConfig(cfg); if (Status_Ok != status) { @@ -182,10 +186,11 @@ int main(int argc, channelP->addIsolatedCallback(secondaryAppCallback); + // // Start streaming - status = channelP->startStreams(Source_Secondary_App_Data); + status = channelP->startStreams(streams); if (Status_Ok != status) { std::cerr << "Failed to start streams: " << Channel::statusString(status) << std::endl; goto clean_out; From c8f0937f1b92109c597964d5d52edb3bb0936da0 Mon Sep 17 00:00:00 2001 From: psmithcrl Date: Tue, 10 Sep 2024 19:16:09 -0400 Subject: [PATCH 05/22] Added new messages and repurposed definitions --- source/LibMultiSense/details/channel.cc | 72 +++++++------ source/LibMultiSense/details/dispatch.cc | 11 +- source/LibMultiSense/details/public.cc | 8 +- .../include/MultiSense/MultiSenseTypes.hh | 9 +- .../MultiSense/details/wire/Protocol.hh | 100 ++++++++++-------- .../wire/SecondaryAppActivateMessage.hh | 85 +++++++++++++++ .../details/wire/SecondaryAppConfigMessage.hh | 13 ++- .../wire/SecondaryAppControlMessage.hh | 14 ++- .../details/wire/SecondaryAppDataMessage.hh | 10 +- .../SecondaryAppGetRegisteredAppsMessage.hh | 75 +++++++++++++ .../wire/SecondaryAppRegisteredAppsMessage.hh | 78 ++++++++++++++ .../SecondaryAppTestUtility.cc | 2 +- 12 files changed, 375 insertions(+), 102 deletions(-) create mode 100644 source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppActivateMessage.hh create mode 100644 source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppGetRegisteredAppsMessage.hh create mode 100644 source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppRegisteredAppsMessage.hh diff --git a/source/LibMultiSense/details/channel.cc b/source/LibMultiSense/details/channel.cc index 88abc0e9..057843a2 100644 --- a/source/LibMultiSense/details/channel.cc +++ b/source/LibMultiSense/details/channel.cc @@ -461,35 +461,35 @@ 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_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_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; @@ -498,7 +498,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) wire_mask |= wire::SOURCE_SECONDARY_APP_DATA; + 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; } @@ -544,7 +549,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) api_mask |= Source_Secondary_App_Data; + 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 8929e5ee..21ef19e0 100644 --- a/source/LibMultiSense/details/dispatch.cc +++ b/source/LibMultiSense/details/dispatch.cc @@ -622,12 +622,11 @@ void impl::dispatch(utility::BufferStreamWriter& buffer) secondary_app::Header header; - header.frameId = SecondaryApp.frameId; - header.source = SecondaryApp.source; - header.bitsPerPixel = SecondaryApp.bitsPerPixel; - header.timeSeconds = SecondaryApp.timeSeconds; - header.timeMicroSeconds = SecondaryApp.timeMicroSeconds; - header.length = SecondaryApp.length; + header.frameId = SecondaryApp.frameId; + header.source = SecondaryApp.source | ((uint64_t)SecondaryApp.sourceExtended << 32); + header.timeSeconds = SecondaryApp.timeSeconds; + header.timeMicroSeconds = SecondaryApp.timeMicroSeconds; + header.length = SecondaryApp.length; header.secondaryAppDataP = SecondaryApp.dataP; dispatchSecondaryApplication(buffer, header); break; diff --git a/source/LibMultiSense/details/public.cc b/source/LibMultiSense/details/public.cc index 0c2cc34b..fba9ade3 100644 --- a/source/LibMultiSense/details/public.cc +++ b/source/LibMultiSense/details/public.cc @@ -1599,7 +1599,9 @@ Status impl::getSecondaryAppConfig(system::SecondaryAppConfig& config) if (Status_Ok != status) return status; - config.framesPerSecond = d.framesPerSecond; + (void) config; + // config.framesPerSecond = d.framesPerSecond; + // TODO: psmith return Status_Ok; } @@ -1614,7 +1616,9 @@ Status impl::getSecondaryAppConfig(system::SecondaryAppConfig& config) Status impl::setSecondaryAppConfig(const system::SecondaryAppConfig& c) { wire::SecondaryAppControl cmd; - cmd.framesPerSecond = c.framesPerSecond; + (void) c; + // cmd.framesPerSecond = c.framesPerSecond; + // TODO: psmith return waitAck(cmd); } diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh index 43e5e8d9..030e3a73 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh @@ -154,7 +154,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 = (1ull<<36); +static CRL_CONSTEXPR DataSource Source_Secondary_App_Data_0 = (1ull<<18); // Same as Source Feature Left +static CRL_CONSTEXPR DataSource Source_Secondary_App_Data_1 = (1ull<<19); // Same as Source Feature Right +static CRL_CONSTEXPR DataSource Source_Secondary_App_Data_2 = (1ull<<32); // Same as Source Feature Aux +static CRL_CONSTEXPR DataSource Source_Secondary_App_Data_3 = (1ull<<33); // Same as Source Feature Rectified Left +static CRL_CONSTEXPR DataSource Source_Secondary_App_Data_4 = (1ull<<34); // Same as Source Feature Rectified Right +static CRL_CONSTEXPR DataSource Source_Secondary_App_Data_5 = (1ull<<35); // Same as Source Feature Rectified Aux /** * Use Roi_Full_Image as the height and width when setting the autoExposureRoi @@ -3083,8 +3088,6 @@ public: /** DataSource corresponding to secondaryAppDataP*/ DataSource source; - /** Bits per pixel in the secondaryAppData */ - uint32_t bitsPerPixel; /** length of the secondaryAppData */ uint32_t length; /** Unique ID used to describe an secondaryAppData. FrameIds increase sequentally from the device */ diff --git a/source/LibMultiSense/include/MultiSense/details/wire/Protocol.hh b/source/LibMultiSense/include/MultiSense/details/wire/Protocol.hh index 8409e2f0..674932f3 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/Protocol.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/Protocol.hh @@ -154,50 +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_SECONDARY_APP_CONTROL = 0x0033; -static CRL_CONSTEXPR IdType ID_CMD_SECONDARY_APP_GET_CONFIG = 0x0034; +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_CONTROL = 0x0033; +static CRL_CONSTEXPR IdType ID_CMD_SECONDARY_APP_GET_CONFIG = 0x0034; +static CRL_CONSTEXPR IdType ID_CMD_SECONDARY_APP_GET_REGISTERED_APPS = 0x0035; +static CRL_CONSTEXPR IdType ID_CMD_SECONDARY_APP_ACTIVATE = 0x0036; + // // Data @@ -242,6 +245,8 @@ 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 = 0x012B; static CRL_CONSTEXPR IdType ID_DATA_SECONDARY_APP_CONFIG = 0x012C; +static CRL_CONSTEXPR IdType ID_DATA_SECONDARY_APP_REGISTERED_APPS = 0x012D; + // // Data sources @@ -288,7 +293,12 @@ 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 = (1ull<<36); // same as SOURCE_RGB_LEFT +static CRL_CONSTEXPR SourceType SOURCE_SECONDARY_APP_DATA_0 = (1ull<<18); // same as SOURCE_FEATURE_LEFT +static CRL_CONSTEXPR SourceType SOURCE_SECONDARY_APP_DATA_1 = (1ull<<19); // same as SOURCE_FEATURE_RIGHT +static CRL_CONSTEXPR SourceType SOURCE_SECONDARY_APP_DATA_2 = (1ull<<32); // same as SOURCE_FEATURE_AUX +static CRL_CONSTEXPR SourceType SOURCE_SECONDARY_APP_DATA_3 = (1ull<<33); // same as SOURCE_FEATURE_RECTIFIED_LEFT +static CRL_CONSTEXPR SourceType SOURCE_SECONDARY_APP_DATA_4 = (1ull<<34); // same as SOURCE_FEATURE_RECTIFIED_RIGHT +static CRL_CONSTEXPR SourceType SOURCE_SECONDARY_APP_DATA_5 = (1ull<<35); // same as SOURCE_FEATURE_RECTIFIED_AUX static CRL_CONSTEXPR SourceType SOURCE_IMAGES = (SOURCE_RAW_LEFT | SOURCE_RAW_RIGHT | 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 00000000..e8176133 --- /dev/null +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppActivateMessage.hh @@ -0,0 +1,85 @@ +/** + * @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() {}; + + // + // 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/SecondaryAppConfigMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppConfigMessage.hh index 122a56a0..4b85f369 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppConfigMessage.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppConfigMessage.hh @@ -54,14 +54,16 @@ public: // // Parameters representing the current camera configuration - float framesPerSecond; + uint32_t dataLength; + uint8_t data[1024]; // // Constructors SecondaryAppConfig(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);}; SecondaryAppConfig(): - framesPerSecond(0.0f) + dataLength(0), + data() {}; // @@ -72,8 +74,11 @@ public: const VersionType version) { (void) version; - message & framesPerSecond; - + message & dataLength; + for (size_t i = 0; i < dataLength; i++) + { + message & data[i]; + } } }; diff --git a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppControlMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppControlMessage.hh index 668918eb..aad96f50 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppControlMessage.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppControlMessage.hh @@ -54,7 +54,9 @@ public: // // Parameters representing the current camera configuration - float framesPerSecond; + uint32_t dataLength; + + uint8_t data[1024]; // @@ -62,7 +64,8 @@ public: SecondaryAppControl(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);}; SecondaryAppControl(): - framesPerSecond(0.0f) + dataLength(0), + data() {}; // @@ -73,8 +76,11 @@ public: const VersionType version) { (void) version; - message & framesPerSecond; - + 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 index 0f3d9e00..178feecd 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppDataMessage.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppDataMessage.hh @@ -59,11 +59,12 @@ static CRL_CONSTEXPR VersionType VERSION = 1; #endif // SENSORPOD_FIRMWARE uint32_t source; - uint32_t bitsPerPixel; uint32_t length; int64_t frameId; uint32_t timeSeconds; uint32_t timeMicroSeconds; + uint32_t sourceExtended; + SecondaryAppHeader() : #ifdef SENSORDPOD_FIRMWARE @@ -71,7 +72,6 @@ static CRL_CONSTEXPR VersionType VERSION = 1; version(VERSION), #endif // SENSORPOD_FIRMWARE source(0), - bitsPerPixel(0), length(0), frameId(0), timeSeconds(0), @@ -101,17 +101,15 @@ public: { (void) version; message & source; - message & bitsPerPixel; message & length; message & frameId; message & timeSeconds; message & timeMicroSeconds; - - const uint32_t dataSize = static_cast (std::ceil(((double) bitsPerPixel / 8.0) * length)); + message & sourceExtended; if (typeid(Archive) == typeid(utility::BufferStreamWriter)) { - message.write(dataP, dataSize); + message.write(dataP, length); } else { 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 00000000..809d01b6 --- /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/SecondaryAppRegisteredAppsMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppRegisteredAppsMessage.hh new file mode 100644 index 00000000..6ed66a63 --- /dev/null +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppRegisteredAppsMessage.hh @@ -0,0 +1,78 @@ +/** + * @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 SecondaryAppRegisteredApps { +public: + static CRL_CONSTEXPR VersionType VERSION = 1; + static CRL_CONSTEXPR IdType ID = ID_CMD_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/SecondaryAppTestUtility/SecondaryAppTestUtility.cc b/source/Utilities/SecondaryAppTestUtility/SecondaryAppTestUtility.cc index 87bfbb39..9fa70c81 100644 --- a/source/Utilities/SecondaryAppTestUtility/SecondaryAppTestUtility.cc +++ b/source/Utilities/SecondaryAppTestUtility/SecondaryAppTestUtility.cc @@ -108,7 +108,7 @@ int main(int argc, std::string currentAddress = "10.66.171.21"; uint32_t mtu = 7200; DataSource streams = Source_Luma_Left; - streams |= Source_Secondary_App_Data; + streams |= Source_Secondary_App_Data_0; image::Config cfg; #if WIN32 From d87a55b339af6448ef6490bd2c624c1a0df28ef1 Mon Sep 17 00:00:00 2001 From: psmithcrl Date: Wed, 11 Sep 2024 15:03:27 -0400 Subject: [PATCH 06/22] added public impls --- source/LibMultiSense/details/public.cc | 73 +++++++++++++++++-- .../include/MultiSense/MultiSenseTypes.hh | 50 ++++++++++++- .../include/MultiSense/details/channel.hh | 3 + .../wire/SecondaryAppActivateMessage.hh | 4 + .../wire/SecondaryAppRegisteredAppsMessage.hh | 8 +- 5 files changed, 126 insertions(+), 12 deletions(-) diff --git a/source/LibMultiSense/details/public.cc b/source/LibMultiSense/details/public.cc index fba9ade3..3a66ceda 100644 --- a/source/LibMultiSense/details/public.cc +++ b/source/LibMultiSense/details/public.cc @@ -119,6 +119,9 @@ #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" namespace crl { namespace multisense { @@ -1599,9 +1602,19 @@ Status impl::getSecondaryAppConfig(system::SecondaryAppConfig& config) if (Status_Ok != status) return status; - (void) config; - // config.framesPerSecond = d.framesPerSecond; - // TODO: psmith + + 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; } @@ -1616,9 +1629,57 @@ Status impl::getSecondaryAppConfig(system::SecondaryAppConfig& config) Status impl::setSecondaryAppConfig(const system::SecondaryAppConfig& c) { wire::SecondaryAppControl cmd; - (void) c; - // cmd.framesPerSecond = c.framesPerSecond; - // TODO: psmith + + 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; + + for (auto app: d.apps) + { + registeredApps.apps.push_back(app); + } + + return Status_Ok; +} + + +// +// Set camera configuration +// +// Currently several sensor messages are combined and presented +// to the user as one. + +Status impl::secondaryAppActivate(const std::string &_name) +{ + wire::SecondaryAppActivate cmd(1, _name); + + return waitAck(cmd); +} + +Status impl::secondaryAppDeactivate(const std::string &_name) +{ + wire::SecondaryAppActivate cmd(0, _name); + return waitAck(cmd); } diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh index 030e3a73..b37ce642 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh @@ -4264,11 +4264,55 @@ struct ChannelStatistics class MULTISENSE_API SecondaryAppConfig { public: - float framesPerSecond; + uint32_t dataLength; + uint8_t data[1024]; SecondaryAppConfig(): - framesPerSecond(5.0f) - {}; + dataLength(0), + data() + {} + + SecondaryAppConfig(const uint32_t _length, const uint8_t * _data): + dataLength(_length) + { + if (_length >= 1024) + { + std::cerr << "Error: Secondary Application Config Length > 1023b" << std::endl; + std::cerr << "Truncating data payload to fit in 1024b\n" << std::endl; + dataLength = 1023; + } + + memcpy(data, _data, dataLength); + } +}; + +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 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 ffcb96a1..10fe4e44 100644 --- a/source/LibMultiSense/include/MultiSense/details/channel.hh +++ b/source/LibMultiSense/include/MultiSense/details/channel.hh @@ -231,6 +231,9 @@ public: virtual Status getSecondaryAppConfig (system::SecondaryAppConfig & c); virtual Status setSecondaryAppConfig (const system::SecondaryAppConfig & c); + virtual Status getRegisteredApps (system::SecondaryAppRegisteredApps & c); + virtual Status secondaryAppActivate (const std::string & s); + virtual Status secondaryAppDeactivate(const std::string & s); virtual Status getFeatureDetectorConfig (system::FeatureDetectorConfig & c); virtual Status setFeatureDetectorConfig (const system::FeatureDetectorConfig & c); diff --git a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppActivateMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppActivateMessage.hh index e8176133..988cfdd2 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppActivateMessage.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppActivateMessage.hh @@ -63,6 +63,10 @@ public: SecondaryAppActivate(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);}; SecondaryAppActivate() {}; + SecondaryAppActivate(const int i, const std::string & s): + activate(i), + name(s) + {} // // Serialization routine. diff --git a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppRegisteredAppsMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppRegisteredAppsMessage.hh index 6ed66a63..256d73ed 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppRegisteredAppsMessage.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppRegisteredAppsMessage.hh @@ -49,7 +49,7 @@ namespace wire { class SecondaryAppRegisteredApps { public: static CRL_CONSTEXPR VersionType VERSION = 1; - static CRL_CONSTEXPR IdType ID = ID_CMD_SECONDARY_APP_REGISTERED_APPS; + static CRL_CONSTEXPR IdType ID = ID_DATA_SECONDARY_APP_REGISTERED_APPS; // // Constructors @@ -68,8 +68,10 @@ public: const VersionType version) { (void) version; - - message & apps; + for (auto app: apps) + { + message & app; + } } }; From 1fba116972b3be8dfb1b27fabca668f30ffc65e5 Mon Sep 17 00:00:00 2001 From: psmithcrl Date: Mon, 16 Sep 2024 16:21:57 -0400 Subject: [PATCH 07/22] Updated the secondaryAppRegistration --- source/LibMultiSense/details/dispatch.cc | 6 +++ source/LibMultiSense/details/public.cc | 3 +- .../include/MultiSense/MultiSenseChannel.hh | 48 +++++++++++++++++++ .../include/MultiSense/MultiSenseTypes.hh | 15 +++++- .../wire/SecondaryAppRegisteredAppsMessage.hh | 24 ++++++++-- 5 files changed, 88 insertions(+), 8 deletions(-) diff --git a/source/LibMultiSense/details/dispatch.cc b/source/LibMultiSense/details/dispatch.cc index 21ef19e0..59a09d30 100644 --- a/source/LibMultiSense/details/dispatch.cc +++ b/source/LibMultiSense/details/dispatch.cc @@ -87,6 +87,9 @@ #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 "MultiSense/details/wire/FeatureDetectorConfigMessage.hh" #include "MultiSense/details/wire/FeatureDetectorGetConfigMessage.hh" @@ -755,6 +758,9 @@ void impl::dispatch(utility::BufferStreamWriter& buffer) 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; diff --git a/source/LibMultiSense/details/public.cc b/source/LibMultiSense/details/public.cc index 3a66ceda..c0ef5aab 100644 --- a/source/LibMultiSense/details/public.cc +++ b/source/LibMultiSense/details/public.cc @@ -1656,7 +1656,8 @@ Status impl::getRegisteredApps(system::SecondaryAppRegisteredApps& registeredApp for (auto app: d.apps) { - registeredApps.apps.push_back(app); + system::SecondaryAppRegisteredApp _a(app.appVersion, app.appName); + registeredApps.apps.push_back(_a); } return Status_Ok; diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh b/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh index 0668cfc2..95d73a10 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh @@ -1118,6 +1118,54 @@ public: */ virtual Status getFeatureDetectorConfig (system::FeatureDetectorConfig& params) = 0; + /** + * Get the secondary application config parameters associated with the MultiSense device + * + * @param params The secondary application parameters to send to the secondary application + * + * @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 setSecondaryAppConfig (const system::SecondaryAppConfig & c) = 0; + + /** + * Query the secondary application(s) registered with the MultiSense device + * + * @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 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 b37ce642..a6d3a96b 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh @@ -4302,15 +4302,26 @@ public: { } }; +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; + std::vector apps; SecondaryAppRegisteredApps(): apps() { } - SecondaryAppRegisteredApps(const std::vector _apps): + SecondaryAppRegisteredApps(const std::vector _apps): apps(_apps) { } }; diff --git a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppRegisteredAppsMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppRegisteredAppsMessage.hh index 256d73ed..4b9bf186 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppRegisteredAppsMessage.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppRegisteredAppsMessage.hh @@ -46,6 +46,23 @@ 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; @@ -58,7 +75,7 @@ public: SecondaryAppRegisteredApps() {}; - std::vector apps; + std::vector apps; // // Serialization routine. @@ -68,10 +85,7 @@ public: const VersionType version) { (void) version; - for (auto app: apps) - { - message & app; - } + message & apps; } }; From 3514cfa6c93059a0dfc9502059616bac718e32e5 Mon Sep 17 00:00:00 2001 From: psmithcrl Date: Thu, 26 Sep 2024 11:30:24 -0400 Subject: [PATCH 08/22] Have feature detector share ids with secondary application --- .../include/MultiSense/details/wire/Protocol.hh | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/source/LibMultiSense/include/MultiSense/details/wire/Protocol.hh b/source/LibMultiSense/include/MultiSense/details/wire/Protocol.hh index 674932f3..57fa6780 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/Protocol.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/Protocol.hh @@ -196,10 +196,10 @@ 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_CONTROL = 0x0033; -static CRL_CONSTEXPR IdType ID_CMD_SECONDARY_APP_GET_CONFIG = 0x0034; -static CRL_CONSTEXPR IdType ID_CMD_SECONDARY_APP_GET_REGISTERED_APPS = 0x0035; -static CRL_CONSTEXPR IdType ID_CMD_SECONDARY_APP_ACTIVATE = 0x0036; +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 @@ -243,9 +243,10 @@ 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 = 0x012B; -static CRL_CONSTEXPR IdType ID_DATA_SECONDARY_APP_CONFIG = 0x012C; -static CRL_CONSTEXPR IdType ID_DATA_SECONDARY_APP_REGISTERED_APPS = 0x012D; +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; // From deae6ec9b40fc94d3d07d930efe07c5cd4450ad9 Mon Sep 17 00:00:00 2001 From: psmithcrl Date: Mon, 7 Oct 2024 13:07:30 -0400 Subject: [PATCH 09/22] Changes required for config and control messages to use secondary app structures --- source/LibMultiSense/details/dispatch.cc | 86 +++++++++---------- source/LibMultiSense/details/public.cc | 27 ++++-- .../wire/FeatureDetectorConfigMessage.hh | 45 +++++----- .../wire/FeatureDetectorControlMessage.hh | 35 +++++--- .../details/wire/SecondaryAppConfigMessage.hh | 6 +- 5 files changed, 116 insertions(+), 83 deletions(-) diff --git a/source/LibMultiSense/details/dispatch.cc b/source/LibMultiSense/details/dispatch.cc index 59a09d30..dff4e5e8 100644 --- a/source/LibMultiSense/details/dispatch.cc +++ b/source/LibMultiSense/details/dispatch.cc @@ -634,46 +634,46 @@ void impl::dispatch(utility::BufferStreamWriter& buffer) dispatchSecondaryApplication(buffer, header); break; } - case MSG_ID(wire::FeatureDetector::ID): - { - wire::FeatureDetector featureDetector(stream, version); - - const wire::FeatureDetectorMeta * metaP = m_featureDetectorMetaCache.find(featureDetector.frameId); - if (NULL == metaP) - break; - - feature_detector::Header header; - header.source = featureDetector.source | ((uint64_t)featureDetector.sourceExtended << 32); - header.frameId = metaP->frameId; - header.timeSeconds = metaP->timeSeconds; - header.timeNanoSeconds= metaP->timeNanoSeconds; - header.ptpNanoSeconds = metaP->ptpNanoSeconds; - header.octaveWidth = metaP->octaveWidth; - header.octaveHeight = metaP->octaveHeight; - header.numOctaves = metaP->numOctaves; - header.scaleFactor = metaP->scaleFactor; - header.motionStatus = metaP->motionStatus; - header.averageXMotion = metaP->averageXMotion; - header.averageYMotion = metaP->averageYMotion; - header.numFeatures = featureDetector.numFeatures; - header.numDescriptors = featureDetector.numDescriptors; - - const size_t startDescriptor=featureDetector.numFeatures*sizeof(wire::Feature); - - uint8_t * dataP = reinterpret_cast(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); - } - - dispatchFeatureDetections(header); - break; - } + // case MSG_ID(wire::FeatureDetector::ID): + // { + // wire::FeatureDetector featureDetector(stream, version); + // + // const wire::FeatureDetectorMeta * metaP = m_featureDetectorMetaCache.find(featureDetector.frameId); + // if (NULL == metaP) + // break; + // + // feature_detector::Header header; + // header.source = featureDetector.source | ((uint64_t)featureDetector.sourceExtended << 32); + // header.frameId = metaP->frameId; + // header.timeSeconds = metaP->timeSeconds; + // header.timeNanoSeconds= metaP->timeNanoSeconds; + // header.ptpNanoSeconds = metaP->ptpNanoSeconds; + // header.octaveWidth = metaP->octaveWidth; + // header.octaveHeight = metaP->octaveHeight; + // header.numOctaves = metaP->numOctaves; + // header.scaleFactor = metaP->scaleFactor; + // header.motionStatus = metaP->motionStatus; + // header.averageXMotion = metaP->averageXMotion; + // header.averageYMotion = metaP->averageYMotion; + // header.numFeatures = featureDetector.numFeatures; + // header.numDescriptors = featureDetector.numDescriptors; + // + // const size_t startDescriptor=featureDetector.numFeatures*sizeof(wire::Feature); + // + // uint8_t * dataP = reinterpret_cast(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); + // } + // + // dispatchFeatureDetections(header); + // break; + // } case MSG_ID(wire::FeatureDetectorMeta::ID): { wire::FeatureDetectorMeta *metaP = new (std::nothrow) wire::FeatureDetectorMeta(stream, version); @@ -764,9 +764,9 @@ void impl::dispatch(utility::BufferStreamWriter& buffer) case MSG_ID(wire::PtpStatusResponse::ID): m_messages.store(wire::PtpStatusResponse(stream, version)); break; - case MSG_ID(wire::FeatureDetectorConfig::ID): - m_messages.store(wire::FeatureDetectorConfig(stream, version)); - break; + // 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 c0ef5aab..ccdbbaea 100644 --- a/source/LibMultiSense/details/public.cc +++ b/source/LibMultiSense/details/public.cc @@ -1686,15 +1686,26 @@ Status impl::secondaryAppDeactivate(const std::string &_name) Status impl::getFeatureDetectorConfig (system::FeatureDetectorConfig & c) { - wire::FeatureDetectorConfig f; + wire::SecondaryAppConfig f; - Status status = waitData(wire::FeatureDetectorGetConfig(), f); + Status status = waitData(wire::SecondaryAppGetConfig(), f); if (Status_Ok != status) return status; - c.setNumberOfFeatures(f.numberOfFeatures); - c.setGrouping(f.grouping); - c.setMotion(f.motion); + + try + { + wire::FeatureDetectorConfig instConfig(f); + c.setNumberOfFeatures(instConfig.configItems.numberOfFeatures); + c.setGrouping(instConfig.configItems.grouping); + c.setMotion(instConfig.configItems.motion); + } + catch (std::runtime_error e) + { + std::cerr << e.what() << std::endl; + return Status_Error; + } + return Status_Ok; } @@ -1702,9 +1713,9 @@ Status impl::setFeatureDetectorConfig (const system::FeatureDetectorConfig & c) { wire::FeatureDetectorControl f; - f.numberOfFeatures = c.numberOfFeatures(); - f.grouping = c.grouping(); - f.motion = c.motion(); + f.controlItems.numberOfFeatures = c.numberOfFeatures(); + f.controlItems.grouping = c.grouping(); + f.controlItems.motion = c.motion(); return waitAck(f); } diff --git a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorConfigMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorConfigMessage.hh index ebcd626e..fac4cab4 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorConfigMessage.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorConfigMessage.hh @@ -41,19 +41,14 @@ #include "MultiSense/details/utility/Portability.hh" #include "MultiSense/details/wire/Protocol.hh" +#include "MultiSense/details/wire/SecondaryAppConfigMessage.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; - - // - // Parameters representing the current camera configuration +struct FeatureDetectorConfigItems { // // The maximum number of features detected per image @@ -70,26 +65,36 @@ public: // Current Octave: 3 uint32_t motion; +}; + +class FeatureDetectorConfig { +public: + static CRL_CONSTEXPR IdType ID = ID_DATA_FEATURE_DETECTOR_CONFIG; + static CRL_CONSTEXPR VersionType VERSION = 1; + // - // Constructors + // Parameters representing the current camera configuration - FeatureDetectorConfig(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);}; - FeatureDetectorConfig() {}; + FeatureDetectorConfigItems configItems; + + SecondaryAppConfig configData; // - // Serialization routine + // Constructors + + FeatureDetectorConfig( SecondaryAppConfig & d ) { - template - void serialize(Archive& message, - const VersionType version) - { + configData = d; + if (d.dataLength != sizeof(configItems)) + { + throw std::runtime_error("Error Invalid receipt length\n"); + } - (void) version; + memcpy(&configItems, d.data, d.dataLength); + + }; + FeatureDetectorConfig() {}; - message & numberOfFeatures; - message & grouping; - message & motion; - } }; }}}} // namespaces diff --git a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorControlMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorControlMessage.hh index 0b9c8422..5fb8d46c 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorControlMessage.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorControlMessage.hh @@ -42,18 +42,14 @@ #include "MultiSense/details/utility/Portability.hh" #include "MultiSense/details/wire/Protocol.hh" +#include "MultiSense/details/wire/SecondaryAppControlMessage.hh" + namespace crl { namespace multisense { namespace details { namespace wire { -class FeatureDetectorControl { -public: - static CRL_CONSTEXPR IdType ID = ID_CMD_FEATURE_DETECTOR_CONTROL; - static CRL_CONSTEXPR VersionType VERSION = 1; - - // - // Parameters representing the current camera configuration +struct FeatureDetectorControlItems { uint32_t numberOfFeatures; @@ -61,10 +57,24 @@ public: uint32_t motion; +} ; + +class FeatureDetectorControl: public SecondaryAppControl { +public: + static CRL_CONSTEXPR IdType ID = ID_CMD_FEATURE_DETECTOR_CONTROL; + static CRL_CONSTEXPR VersionType VERSION = 1; + + // + // Parameters representing the current camera configuration + FeatureDetectorControlItems controlItems; + // // Constructors - FeatureDetectorControl(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);}; + FeatureDetectorControl(utility::BufferStreamReader&r, VersionType v) { + SecondaryAppControl::serialize(r,v); + memcpy(&controlItems, data, dataLength); + }; FeatureDetectorControl() {}; // @@ -76,9 +86,12 @@ public: { (void) version; - message & numberOfFeatures; - message & grouping; - message & motion; + + memset(data, 0, sizeof(data)); + dataLength = sizeof(FeatureDetectorControlItems); + memcpy(data, &controlItems, dataLength); + + SecondaryAppControl::serialize(message, version); } }; diff --git a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppConfigMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppConfigMessage.hh index 4b85f369..dcab2ab2 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppConfigMessage.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppConfigMessage.hh @@ -60,7 +60,10 @@ public: // // Constructors - SecondaryAppConfig(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);}; + SecondaryAppConfig(utility::BufferStreamReader&r, VersionType v) { + serialize(r,v); + }; + SecondaryAppConfig(): dataLength(0), data() @@ -75,6 +78,7 @@ public: { (void) version; message & dataLength; + for (size_t i = 0; i < dataLength; i++) { message & data[i]; From d3e87571c84c92768115fd1f3244a091a30d6f74 Mon Sep 17 00:00:00 2001 From: psmithcrl Date: Thu, 31 Oct 2024 14:18:23 -0400 Subject: [PATCH 10/22] Added metadata message for generic metadata transport --- .../details/wire/SecondaryAppMetaMessage.hh | 110 ++++++++++++++++++ 1 file changed, 110 insertions(+) create mode 100644 source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh 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 00000000..198660c7 --- /dev/null +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh @@ -0,0 +1,110 @@ +/** + * @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 + size_t dataLength; + + SecondaryAppMetaHeader(): +#ifdef SENSORDPOD_FIRMWARE + id(ID), + version(VERSION), +#endif // SENSORPOD_FIRMWARE + dataLength(0) + {}; +}; + +#ifndef SENSORPOD_FIRMWARE + +class SecondaryAppMetadata : public SecondaryAppMetaHeader { +public: + + void * dataP; + + // + // Constructors + + SecondaryAppMetadata(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);}; + SecondaryAppMetadata() : dataLength(0), dataP(NULL) {}; + + // + // Serialization routine + + template + void serialize(Archive& message, + const VersionType version) + { + (void) version; + + if (typeid(Archive) == typeid(utility::BufferStreamWriter)) { + + message.write(dataP, dataLength); + + } else { + + dataP = message.peek(); + message.seek(message.tell() + dataLength); + } + + } +}; + +#endif // !SENSORPOD_FIRMWARE + +}}}} // namespaces + +#endif From b769dbdc501b36ba4a52d0d52cb4bc6bdd1e389c Mon Sep 17 00:00:00 2001 From: psmithcrl Date: Mon, 4 Nov 2024 12:56:25 -0500 Subject: [PATCH 11/22] Move the dataP into the metadata header --- .../MultiSense/details/wire/SecondaryAppMetaMessage.hh | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh index 198660c7..5d63be0e 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh @@ -57,14 +57,17 @@ static CRL_CONSTEXPR VersionType VERSION = 1; IdType id; VersionType version; #endif // SENSORPOD_FIRMWARE + size_t dataLength; + void * dataP; SecondaryAppMetaHeader(): #ifdef SENSORDPOD_FIRMWARE id(ID), version(VERSION), #endif // SENSORPOD_FIRMWARE - dataLength(0) + dataLength(0), + dataP(nullptr) {}; }; @@ -73,7 +76,6 @@ static CRL_CONSTEXPR VersionType VERSION = 1; class SecondaryAppMetadata : public SecondaryAppMetaHeader { public: - void * dataP; // // Constructors From 66d93920a6052761767ac89c7bd1f1a0f5fcb25c Mon Sep 17 00:00:00 2001 From: psmithcrl Date: Mon, 4 Nov 2024 15:13:40 -0500 Subject: [PATCH 12/22] Added mask for all Secondary application streams --- .../include/MultiSense/details/wire/Protocol.hh | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/source/LibMultiSense/include/MultiSense/details/wire/Protocol.hh b/source/LibMultiSense/include/MultiSense/details/wire/Protocol.hh index 57fa6780..4aef0441 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/Protocol.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/Protocol.hh @@ -301,6 +301,13 @@ static CRL_CONSTEXPR SourceType SOURCE_SECONDARY_APP_DATA_3 = (1ull<<33); static CRL_CONSTEXPR SourceType SOURCE_SECONDARY_APP_DATA_4 = (1ull<<34); // same as SOURCE_FEATURE_RECTIFIED_RIGHT static CRL_CONSTEXPR SourceType SOURCE_SECONDARY_APP_DATA_5 = (1ull<<35); // same as SOURCE_FEATURE_RECTIFIED_AUX +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 | SOURCE_RAW_AUX | From 214902b17a5a03b5fee857fedba5d19208a28c69 Mon Sep 17 00:00:00 2001 From: psmithcrl Date: Mon, 11 Nov 2024 12:40:07 -0500 Subject: [PATCH 13/22] Purged some feature detector data, added frameId to metadata --- source/LibMultiSense/details/channel.cc | 17 ++-- source/LibMultiSense/details/dispatch.cc | 39 +++++---- source/LibMultiSense/details/public.cc | 86 +++++++++---------- .../include/MultiSense/MultiSenseChannel.hh | 6 +- .../include/MultiSense/MultiSenseTypes.hh | 7 +- .../include/MultiSense/details/channel.hh | 18 ++-- .../details/wire/SecondaryAppMetaMessage.hh | 8 +- 7 files changed, 98 insertions(+), 83 deletions(-) diff --git a/source/LibMultiSense/details/channel.cc b/source/LibMultiSense/details/channel.cc index 057843a2..3f9184bc 100644 --- a/source/LibMultiSense/details/channel.cc +++ b/source/LibMultiSense/details/channel.cc @@ -79,7 +79,8 @@ 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_featureDetectorMetaCache(FEATURE_DETECTOR_META_CACHE_DEPTH), + m_secondaryAppMetaCache(SECONDARY_APP_META_CACHE_DEPTH), m_udpAssemblerMap(), m_dispatchLock(), m_streamLock(), @@ -93,7 +94,7 @@ impl::impl(const std::string& address, const RemoteHeadChannel& cameraId, const m_imuListeners(), m_compressedImageListeners(), m_secondaryAppListeners(), - m_featureDetectorListeners(), + // m_featureDetectorListeners(), m_watch(), m_messages(), m_streamsEnabled(0), @@ -270,11 +271,11 @@ void impl::cleanup() its != m_secondaryAppListeners.end(); its ++) delete *its; - std::list::const_iterator itf; - for(itf = m_featureDetectorListeners.begin(); - itf != m_featureDetectorListeners.end(); - ++ itf) - delete *itf; + // std::list::const_iterator itf; + // for(itf = m_featureDetectorListeners.begin(); + // itf != m_featureDetectorListeners.end(); + // ++ itf) + // delete *itf; BufferPool::const_iterator it; for(it = m_rxLargeBufferPool.begin(); it != m_rxLargeBufferPool.end(); @@ -291,7 +292,7 @@ void impl::cleanup() m_imuListeners.clear(); m_secondaryAppListeners.clear(); m_compressedImageListeners.clear(); - m_featureDetectorListeners.clear(); + // m_featureDetectorListeners.clear(); m_rxLargeBufferPool.clear(); m_rxSmallBufferPool.clear(); diff --git a/source/LibMultiSense/details/dispatch.cc b/source/LibMultiSense/details/dispatch.cc index dff4e5e8..308f9363 100644 --- a/source/LibMultiSense/details/dispatch.cc +++ b/source/LibMultiSense/details/dispatch.cc @@ -253,20 +253,20 @@ void impl::dispatchAprilTagDetections(apriltag::Header& header) // // Publish a feature detection event -void impl::dispatchFeatureDetections(feature_detector::Header& header) -{ - utility::ScopedLock lock(m_dispatchLock); - - std::list::const_iterator it; - - for(it = m_featureDetectorListeners.begin(); - it != m_featureDetectorListeners.end(); - ++ it) - (*it)->dispatch(header); - - utility::ScopedLock statsLock(m_statisticsLock); - m_channelStatistics.numDispatchedFeatureDetections++; -} +// void impl::dispatchFeatureDetections(feature_detector::Header& header) +// { +// utility::ScopedLock lock(m_dispatchLock); +// +// std::list::const_iterator it; +// +// for(it = m_featureDetectorListeners.begin(); +// it != m_featureDetectorListeners.end(); +// ++ it) +// (*it)->dispatch(header); +// +// utility::ScopedLock statsLock(m_statisticsLock); +// m_channelStatistics.numDispatchedFeatureDetections++; +// } // // Publish Secondary App Data @@ -282,6 +282,9 @@ void impl::dispatchSecondaryApplication(utility::BufferStream& buffer, it != m_secondaryAppListeners.end(); it ++) (*it)->dispatch(buffer, header); + + utility::ScopedLock statsLock(m_statisticsLock); + m_channelStatistics.numDispatchedSecondary++; } @@ -621,6 +624,7 @@ void impl::dispatch(utility::BufferStreamWriter& buffer) } case MSG_ID(wire::SecondaryAppData::ID): { + printf("[%s] secondaryAppData\n", __func__ ); wire::SecondaryAppData SecondaryApp(stream, version); secondary_app::Header header; @@ -674,14 +678,15 @@ void impl::dispatch(utility::BufferStreamWriter& buffer) // dispatchFeatureDetections(header); // break; // } - case MSG_ID(wire::FeatureDetectorMeta::ID): + case MSG_ID(wire::SecondaryAppMetadata::ID): { - wire::FeatureDetectorMeta *metaP = new (std::nothrow) wire::FeatureDetectorMeta(stream, version); + printf("[%s] secondaryAppMetaData\n", __func__ ); + 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): diff --git a/source/LibMultiSense/details/public.cc b/source/LibMultiSense/details/public.cc index ccdbbaea..933d44a4 100644 --- a/source/LibMultiSense/details/public.cc +++ b/source/LibMultiSense/details/public.cc @@ -290,24 +290,24 @@ Status impl::addIsolatedCallback(apriltag::Callback callback, // // 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; -} +// +// 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 @@ -555,31 +555,31 @@ Status impl::removeIsolatedCallback(secondary_app::Callback callback) // // Removes a feature detector listener - -Status impl::removeIsolatedCallback(feature_detector::Callback callback) -{ - try { - utility::ScopedLock lock(m_dispatchLock); - - std::list::iterator it; - for(it = m_featureDetectorListeners.begin(); - it != m_featureDetectorListeners.end(); - ++ it) { - - if ((*it)->callback() == callback) { - delete *it; - m_featureDetectorListeners.erase(it); - return Status_Ok; - } - } - - } catch (const std::exception& e) { - CRL_DEBUG("exception: %s\n", e.what()); - return Status_Exception; - } - - return Status_Error; -} +// +// Status impl::removeIsolatedCallback(feature_detector::Callback callback) +// { +// try { +// utility::ScopedLock lock(m_dispatchLock); +// +// std::list::iterator it; +// for(it = m_featureDetectorListeners.begin(); +// it != m_featureDetectorListeners.end(); +// ++ it) { +// +// if ((*it)->callback() == callback) { +// delete *it; +// m_featureDetectorListeners.erase(it); +// return Status_Ok; +// } +// } +// +// } catch (const std::exception& e) { +// CRL_DEBUG("exception: %s\n", e.what()); +// return Status_Exception; +// } +// +// return Status_Error; +// } // // Reserve the current callback buffer being used in a dispatch thread diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh b/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh index 95d73a10..10ea677e 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh @@ -390,8 +390,8 @@ public: * @return A crl::multisense::Status indicating if the callback registration * succeeded or failed */ - virtual Status addIsolatedCallback(feature_detector::Callback callback, - void *userDataP=NULL) = 0; + // virtual Status addIsolatedCallback(feature_detector::Callback callback, + // void *userDataP=NULL) = 0; /** * Unregister a user defined image::Callback. This stops the callback @@ -487,7 +487,7 @@ public: * succeeded or failed */ - virtual Status removeIsolatedCallback(feature_detector::Callback callback) = 0; + // virtual Status removeIsolatedCallback(feature_detector::Callback callback) = 0; /** * Unregister a user defined secondary_app::Callback. This stops the callback diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh index a6d3a96b..97322221 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh @@ -4207,7 +4207,8 @@ struct ChannelStatistics numDispatchedImu(0), numDispatchedCompressedImage(0), numDispatchedGroundSurfaceSpline(0), - numDispatchedAprilTagDetections(0) + numDispatchedAprilTagDetections(0), + numDispatchedSecondary(0) { }; @@ -4258,8 +4259,8 @@ 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 { diff --git a/source/LibMultiSense/include/MultiSense/details/channel.hh b/source/LibMultiSense/include/MultiSense/details/channel.hh index 10fe4e44..f79e959c 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" @@ -126,8 +126,8 @@ public: virtual Status addIsolatedCallback (secondary_app::Callback callback, void *userDataP); - virtual Status addIsolatedCallback (feature_detector::Callback callback, - void *userDataP); + // virtual Status addIsolatedCallback (feature_detector::Callback callback, + // void *userDataP); virtual Status removeIsolatedCallback(image::Callback callback); virtual Status removeIsolatedCallback(lidar::Callback callback); @@ -137,7 +137,7 @@ public: virtual Status removeIsolatedCallback(ground_surface::Callback callback); virtual Status removeIsolatedCallback(apriltag::Callback callback); virtual Status removeIsolatedCallback(secondary_app::Callback callback); - virtual Status removeIsolatedCallback(feature_detector::Callback callback); + // virtual Status removeIsolatedCallback(feature_detector::Callback callback); virtual void* reserveCallbackBuffer (); virtual Status releaseCallbackBuffer (void *referenceP); @@ -311,6 +311,7 @@ private: 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 SECONDARY_APP_META_CACHE_DEPTH = 4; static CRL_CONSTEXPR uint32_t FEATURE_DETECTOR_META_CACHE_DEPTH = 4; static CRL_CONSTEXPR uint32_t UDP_TRACKER_CACHE_DEPTH = 4; static CRL_CONSTEXPR uint32_t TIME_SYNC_OFFSET_DECAY = 8; @@ -457,7 +458,12 @@ private: // // A cache of feature detector meta data - DepthCache m_featureDetectorMetaCache; + // DepthCache m_featureDetectorMetaCache; + + // + // A cache of secondary application meta data + + DepthCache m_secondaryAppMetaCache; // // A map of custom UDP assemblers @@ -503,7 +509,7 @@ private: std::list m_groundSurfaceSplineListeners; std::list m_aprilTagDetectionListeners; std::list m_secondaryAppListeners; - std::list m_featureDetectorListeners; + // std::list m_featureDetectorListeners; // // A message signal interface diff --git a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh index 5d63be0e..177f052a 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh @@ -58,14 +58,16 @@ static CRL_CONSTEXPR VersionType VERSION = 1; VersionType version; #endif // SENSORPOD_FIRMWARE - size_t dataLength; - void * dataP; + 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) {}; @@ -81,7 +83,7 @@ public: // Constructors SecondaryAppMetadata(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);}; - SecondaryAppMetadata() : dataLength(0), dataP(NULL) {}; + SecondaryAppMetadata() {}; // // Serialization routine From fd7ce1726c34e7cd24da69427ef429ea8f65ed56 Mon Sep 17 00:00:00 2001 From: psmithcrl Date: Mon, 11 Nov 2024 19:37:20 -0500 Subject: [PATCH 14/22] Some changes to support converting generic payload to defined payload --- source/LibMultiSense/details/dispatch.cc | 60 ++++-------- source/LibMultiSense/details/public.cc | 79 ++++++++++----- .../include/MultiSense/MultiSenseChannel.hh | 13 +++ .../include/MultiSense/details/channel.hh | 5 + .../details/wire/SecondaryAppMetaMessage.hh | 2 + .../FeatureDetectorUtility.cc | 98 +++++++++++++------ 6 files changed, 158 insertions(+), 99 deletions(-) diff --git a/source/LibMultiSense/details/dispatch.cc b/source/LibMultiSense/details/dispatch.cc index 308f9363..c232e6d8 100644 --- a/source/LibMultiSense/details/dispatch.cc +++ b/source/LibMultiSense/details/dispatch.cc @@ -624,7 +624,6 @@ void impl::dispatch(utility::BufferStreamWriter& buffer) } case MSG_ID(wire::SecondaryAppData::ID): { - printf("[%s] secondaryAppData\n", __func__ ); wire::SecondaryAppData SecondaryApp(stream, version); secondary_app::Header header; @@ -638,49 +637,8 @@ void impl::dispatch(utility::BufferStreamWriter& buffer) dispatchSecondaryApplication(buffer, header); break; } - // case MSG_ID(wire::FeatureDetector::ID): - // { - // wire::FeatureDetector featureDetector(stream, version); - // - // const wire::FeatureDetectorMeta * metaP = m_featureDetectorMetaCache.find(featureDetector.frameId); - // if (NULL == metaP) - // break; - // - // feature_detector::Header header; - // header.source = featureDetector.source | ((uint64_t)featureDetector.sourceExtended << 32); - // header.frameId = metaP->frameId; - // header.timeSeconds = metaP->timeSeconds; - // header.timeNanoSeconds= metaP->timeNanoSeconds; - // header.ptpNanoSeconds = metaP->ptpNanoSeconds; - // header.octaveWidth = metaP->octaveWidth; - // header.octaveHeight = metaP->octaveHeight; - // header.numOctaves = metaP->numOctaves; - // header.scaleFactor = metaP->scaleFactor; - // header.motionStatus = metaP->motionStatus; - // header.averageXMotion = metaP->averageXMotion; - // header.averageYMotion = metaP->averageYMotion; - // header.numFeatures = featureDetector.numFeatures; - // header.numDescriptors = featureDetector.numDescriptors; - // - // const size_t startDescriptor=featureDetector.numFeatures*sizeof(wire::Feature); - // - // uint8_t * dataP = reinterpret_cast(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); - // } - // - // dispatchFeatureDetections(header); - // break; - // } case MSG_ID(wire::SecondaryAppMetadata::ID): { - printf("[%s] secondaryAppMetaData\n", __func__ ); wire::SecondaryAppMetadata *metaP = new (std::nothrow) wire::SecondaryAppMetadata(stream, version); if (NULL == metaP) @@ -796,6 +754,24 @@ void impl::dispatch(utility::BufferStreamWriter& buffer) } } +Status impl::findMetadata(wire::FeatureDetectorMeta * f, const int64_t & frameId) +{ + wire::SecondaryAppMetadata * m = m_secondaryAppMetaCache.find(frameId); + + if (NULL == m) { + fprintf(stderr, "%s Unable to find metadata for frame: %ld\n", __func__, frameId); + return Status_Error; + } + + utility::BufferStreamReader stream(reinterpret_cast(m->dataP), m->dataLength); + + wire::FeatureDetectorMeta _f(stream, 1); + memcpy(f, &_f, sizeof(_f)); + + return Status_Ok; +} + + // // Get a UDP assembler for this message type. We are given // the first UDP packet in the stream diff --git a/source/LibMultiSense/details/public.cc b/source/LibMultiSense/details/public.cc index 933d44a4..4e2f2339 100644 --- a/source/LibMultiSense/details/public.cc +++ b/source/LibMultiSense/details/public.cc @@ -123,6 +123,8 @@ #include "MultiSense/details/wire/SecondaryAppRegisteredAppsMessage.hh" #include "MultiSense/details/wire/SecondaryAppActivateMessage.hh" +#include "MultiSense/details/utility/BufferStream.hh" + namespace crl { namespace multisense { namespace details { @@ -553,33 +555,56 @@ Status impl::removeIsolatedCallback(secondary_app::Callback callback) return Status_Error; } -// -// Removes a feature detector listener -// -// Status impl::removeIsolatedCallback(feature_detector::Callback callback) -// { -// try { -// utility::ScopedLock lock(m_dispatchLock); -// -// std::list::iterator it; -// for(it = m_featureDetectorListeners.begin(); -// it != m_featureDetectorListeners.end(); -// ++ it) { -// -// if ((*it)->callback() == callback) { -// delete *it; -// m_featureDetectorListeners.erase(it); -// return Status_Ok; -// } -// } -// -// } catch (const std::exception& e) { -// CRL_DEBUG("exception: %s\n", e.what()); -// return Status_Exception; -// } -// -// return Status_Error; -// } +Status impl::secondaryAppDataExtract(feature_detector::Header &header, const uint8_t * data, const size_t len, const int64_t frameId) +{ + utility::BufferStreamReader stream(data, len); + wire::FeatureDetector featureDetector(stream, 1); //TODO + + + wire::FeatureDetectorMeta m; + if (findMetadata(&m, frameId) == Status_Ok) + { + fprintf(stderr, "%s Meta data extracted\n", __func__ ); + } + else + { + fprintf(stderr, "%s Error metadata extraction for frame %ld failed\n", __func__, frameId ); + return Status_Error; + } + + header.source = featureDetector.source | ((uint64_t)featureDetector.sourceExtended << 32); + header.frameId = m.frameId; + header.timeSeconds = m.timeSeconds; + header.timeNanoSeconds= m.timeNanoSeconds; + header.ptpNanoSeconds = m.ptpNanoSeconds; + header.octaveWidth = m.octaveWidth; + header.octaveHeight = m.octaveHeight; + header.numOctaves = m.numOctaves; + header.scaleFactor = m.scaleFactor; + header.motionStatus = m.motionStatus; + header.averageXMotion = m.averageXMotion; + header.averageYMotion = m.averageYMotion; + header.numFeatures = featureDetector.numFeatures; + header.numDescriptors = featureDetector.numDescriptors; + + const size_t startDescriptor=featureDetector.numFeatures*sizeof(wire::Feature); + + uint8_t * dataP = reinterpret_cast(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; +} + + + // // Reserve the current callback buffer being used in a dispatch thread diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh b/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh index 10ea677e..fc9b5364 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh @@ -500,6 +500,19 @@ public: */ virtual Status removeIsolatedCallback(secondary_app::Callback callback) = 0; + /** + * Provides an interface to convert generic secondary app data to usable feature detector data + * + * @param h the application defined feature detector header + * @param data the generic payload from the secondary application + * @param len the length of the generic payload + * @param frameId the frameId for metadata lookup + * + * @return A crl::multisense::Status indicating if the callback deregistration + * succeeded or failed + */ + virtual Status secondaryAppDataExtract(feature_detector::Header &h, const uint8_t * data, const size_t len, const int64_t frameId) = 0; + /** * Reserve image or lidar data within a isolated callback so it is available * after the callback returns. diff --git a/source/LibMultiSense/include/MultiSense/details/channel.hh b/source/LibMultiSense/include/MultiSense/details/channel.hh index f79e959c..088f2884 100644 --- a/source/LibMultiSense/include/MultiSense/details/channel.hh +++ b/source/LibMultiSense/include/MultiSense/details/channel.hh @@ -49,6 +49,7 @@ #include "MultiSense/details/wire/Protocol.hh" #include "MultiSense/details/wire/ImageMetaMessage.hh" #include "MultiSense/details/wire/SecondaryAppMetaMessage.hh" +#include "MultiSense/details/wire/FeatureDetectorMetaMessage.hh" #include "MultiSense/details/wire/StatusResponseMessage.hh" #include "MultiSense/details/wire/PtpStatusResponseMessage.hh" #include "MultiSense/details/wire/VersionResponseMessage.hh" @@ -103,6 +104,8 @@ public: // // Public API + virtual Status findMetadata (wire::FeatureDetectorMeta * f, const int64_t & frameId); + virtual Status addIsolatedCallback (image::Callback callback, DataSource imageSourceMask, void *userDataP); @@ -139,6 +142,8 @@ public: virtual Status removeIsolatedCallback(secondary_app::Callback callback); // virtual Status removeIsolatedCallback(feature_detector::Callback callback); + virtual Status secondaryAppDataExtract(feature_detector::Header &h, const uint8_t * data, const size_t len, const int64_t frameId); + virtual void* reserveCallbackBuffer (); virtual Status releaseCallbackBuffer (void *referenceP); diff --git a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh index 177f052a..7e13d70e 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh @@ -99,6 +99,8 @@ public: message.write(dataP, dataLength); } else { + message & frameId; + message & dataLength; dataP = message.peek(); message.seek(message.tell() + dataLength); diff --git a/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc b/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc index 7fd1cdf6..3a312aeb 100644 --- a/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc +++ b/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc @@ -345,43 +345,50 @@ void imageCallback(const image::Header& header, std::cerr << "failed to get histogram for frame " << header.frameId << std::endl; } -void featureDetectorCallback(const feature_detector::Header& header, +void featureDetectorCallback(const secondary_app::Header& header, void *userDataP) { + feature_detector::Header fHeader; UserData *userData = reinterpret_cast(userDataP); + Status s = userData->channelP->secondaryAppDataExtract(fHeader, reinterpret_cast(header.secondaryAppDataP), header.secondaryAppDataLength, header.frameId); + if (s != Status_Ok) + { + fprintf(stderr, "%s Error failed extraction\n", __func__ ); + return; + } - if ((header.source == Source_Feature_Left) - || (header.source == Source_Feature_Rectified_Left)) { + if ((fHeader.source == Source_Feature_Left) + || (fHeader.source == Source_Feature_Rectified_Left)) { - auto it = userData->elapsedTime.find(header.frameId); + auto it = userData->elapsedTime.find(fHeader.frameId); if (it == userData->elapsedTime.end()) { - std::cout << "Unexpected result, image not yet received for frame: " << header.frameId << "\n"; + std::cout << "Unexpected result, image not yet received for frame: " << fHeader.frameId << "\n"; featureDetectionTime t; t.featureTime = std::chrono::system_clock::now(); - userData->elapsedTime.insert(std::pair(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 + 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"; } } // anonymous @@ -496,6 +503,37 @@ int main(int argc, } { + + system::SecondaryAppRegisteredApps s; + status = channelP->getRegisteredApps(s); + if (Status_Ok != status) + { + std::cerr << "Error failed to get registered apps\n"; + return -2; + } + + status = channelP->secondaryAppActivate(s.apps[0].appName); + if (Status_Ok != status) + { + std::cerr << "Error failed to activate app " << s.apps[0].appName; + return -2; + } + + fprintf(stderr, "%s got registered app: %s activated\n", __func__, s.apps[0].appName.c_str() ); + + system::FeatureDetectorConfig c; + c.setNumberOfFeatures(5000); + c.setGrouping(1); + c.setMotion(0); + status = channelP->setFeatureDetectorConfig(c); + if (Status_Ok != status) + { + std::cerr << "Error failed to set featureDetectorConfig apps\n"; + return -2; + } + + std::cout << "Successfully Configured Feature Detector\n"; + system::FeatureDetectorConfig fcfg; status = channelP->getFeatureDetectorConfig(fcfg); @@ -585,18 +623,18 @@ int main(int argc, system::StatusMessage statusMessage; status = channelP->getDeviceStatus(statusMessage); - if (Status_Ok == status) { - std::cout << "Uptime: " << statusMessage.uptime << ", " << - "SystemOk: " << statusMessage.systemOk << ", " << - "FPGA Temp: " << statusMessage.fpgaTemperature << ", " << - "Left Imager Temp: " << statusMessage.leftImagerTemperature << ", " << - "Right Imager Temp: " << statusMessage.rightImagerTemperature << ", " << - "Input Voltage: " << statusMessage.inputVoltage << ", " << - "Input Current: " << statusMessage.inputCurrent << ", " << - "FPGA Power: " << statusMessage.fpgaPower << ", " << - "Logic Power: " << statusMessage.logicPower << ", " << - "Imager Power: " << statusMessage.imagerPower << std::endl; - } + // if (Status_Ok == status) { + // std::cout << "Uptime: " << statusMessage.uptime << ", " << + // "SystemOk: " << statusMessage.systemOk << ", " << + // "FPGA Temp: " << statusMessage.fpgaTemperature << ", " << + // "Left Imager Temp: " << statusMessage.leftImagerTemperature << ", " << + // "Right Imager Temp: " << statusMessage.rightImagerTemperature << ", " << + // "Input Voltage: " << statusMessage.inputVoltage << ", " << + // "Input Current: " << statusMessage.inputCurrent << ", " << + // "FPGA Power: " << statusMessage.fpgaPower << ", " << + // "Logic Power: " << statusMessage.logicPower << ", " << + // "Imager Power: " << statusMessage.imagerPower << std::endl; + // } usleep(1000000); } From 2861b0e5f269e99f516cf4e77ca34167c06f851e Mon Sep 17 00:00:00 2001 From: psmithcrl Date: Wed, 13 Nov 2024 12:35:38 -0500 Subject: [PATCH 15/22] fixed up some messages --- source/LibMultiSense/details/public.cc | 3 ++- .../details/wire/FeatureDetectorMessage.hh | 17 +++-------------- .../details/wire/FeatureDetectorMetaMessage.hh | 11 +++-------- 3 files changed, 8 insertions(+), 23 deletions(-) diff --git a/source/LibMultiSense/details/public.cc b/source/LibMultiSense/details/public.cc index 4e2f2339..0d3d6173 100644 --- a/source/LibMultiSense/details/public.cc +++ b/source/LibMultiSense/details/public.cc @@ -572,7 +572,8 @@ Status impl::secondaryAppDataExtract(feature_detector::Header &header, const uin return Status_Error; } - header.source = featureDetector.source | ((uint64_t)featureDetector.sourceExtended << 32); + + header.source = featureDetector.source; header.frameId = m.frameId; header.timeSeconds = m.timeSeconds; header.timeNanoSeconds= m.timeNanoSeconds; diff --git a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorMessage.hh index dbf1286a..97dd19ad 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorMessage.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorMessage.hh @@ -96,18 +96,16 @@ public: class WIRE_HEADER_ATTRIBS_ FeatureDetectorHeader { public: static CRL_CONSTEXPR IdType ID = ID_DATA_FEATURE_DETECTOR; - static CRL_CONSTEXPR VersionType VERSION = 2; + static CRL_CONSTEXPR VersionType VERSION = 1; #ifdef SENSORPOD_FIRMWARE IdType id; VersionType version; #endif // SENSORPOD_FIRMWARE - uint32_t source; + uint64_t source; int64_t frameId; uint16_t numFeatures; uint16_t numDescriptors; - uint32_t sourceExtended; - FeatureDetectorHeader() : #ifdef SENSORPOD_FIRMWARE @@ -117,8 +115,7 @@ public: source(0), frameId(0), numFeatures(0), - numDescriptors(0), - sourceExtended(0) + numDescriptors(0) {}; }; @@ -164,14 +161,6 @@ public: message.seek(message.tell() + featureDataSize); } - if (version >= 2) - { - message & sourceExtended; - } - else - { - sourceExtended = 0; - } } }; diff --git a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorMetaMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorMetaMessage.hh index da7e0062..652ac5a8 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorMetaMessage.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorMetaMessage.hh @@ -50,10 +50,7 @@ namespace wire { 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 uint32_t length; uint32_t source; int64_t frameId; @@ -71,10 +68,7 @@ namespace wire { uint16_t numDescriptors; FeatureDetectorMetaHeader() : - #ifdef SENSORPOD_FIRMWARE - id(ID), version(VERSION), - #endif // SENSORPOD_FIRMWARE length(0), source(0), frameId(0), @@ -110,9 +104,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; From b9f0488e206fdfc31fe820d212a4e007895000fe Mon Sep 17 00:00:00 2001 From: psmithcrl Date: Mon, 18 Nov 2024 12:36:23 -0500 Subject: [PATCH 16/22] Capture metadata buffer reference, auto release in depth cache pop --- source/LibMultiSense/details/dispatch.cc | 36 ++++++---------- source/LibMultiSense/details/public.cc | 41 ++++++++----------- .../include/MultiSense/MultiSenseChannel.hh | 2 +- .../include/MultiSense/MultiSenseTypes.hh | 4 ++ .../include/MultiSense/details/channel.hh | 11 +---- .../details/wire/SecondaryAppMetaMessage.hh | 13 +++++- .../FeatureDetectorUtility.cc | 5 ++- 7 files changed, 51 insertions(+), 61 deletions(-) diff --git a/source/LibMultiSense/details/dispatch.cc b/source/LibMultiSense/details/dispatch.cc index c232e6d8..746c3c08 100644 --- a/source/LibMultiSense/details/dispatch.cc +++ b/source/LibMultiSense/details/dispatch.cc @@ -628,12 +628,18 @@ void impl::dispatch(utility::BufferStreamWriter& buffer) secondary_app::Header header; - header.frameId = SecondaryApp.frameId; - header.source = SecondaryApp.source | ((uint64_t)SecondaryApp.sourceExtended << 32); - header.timeSeconds = SecondaryApp.timeSeconds; - header.timeMicroSeconds = SecondaryApp.timeMicroSeconds; - header.length = SecondaryApp.length; - header.secondaryAppDataP = SecondaryApp.dataP; + wire::SecondaryAppMetadata * metaP = m_secondaryAppMetaCache.find(SecondaryApp.frameId); + if (metaP == NULL) + break; + + header.frameId = SecondaryApp.frameId; + header.source = SecondaryApp.source | ((uint64_t)SecondaryApp.sourceExtended << 32); + header.timeSeconds = SecondaryApp.timeSeconds; + header.timeMicroSeconds = SecondaryApp.timeMicroSeconds; + header.length = SecondaryApp.length; + header.secondaryAppDataP = SecondaryApp.dataP; + header.secondaryAppMetadataP = metaP->dataP; + header.secondaryAppMetadataLength = metaP->dataLength; dispatchSecondaryApplication(buffer, header); break; } @@ -754,24 +760,6 @@ void impl::dispatch(utility::BufferStreamWriter& buffer) } } -Status impl::findMetadata(wire::FeatureDetectorMeta * f, const int64_t & frameId) -{ - wire::SecondaryAppMetadata * m = m_secondaryAppMetaCache.find(frameId); - - if (NULL == m) { - fprintf(stderr, "%s Unable to find metadata for frame: %ld\n", __func__, frameId); - return Status_Error; - } - - utility::BufferStreamReader stream(reinterpret_cast(m->dataP), m->dataLength); - - wire::FeatureDetectorMeta _f(stream, 1); - memcpy(f, &_f, sizeof(_f)); - - return Status_Ok; -} - - // // Get a UDP assembler for this message type. We are given // the first UDP packet in the stream diff --git a/source/LibMultiSense/details/public.cc b/source/LibMultiSense/details/public.cc index 0d3d6173..31b516f7 100644 --- a/source/LibMultiSense/details/public.cc +++ b/source/LibMultiSense/details/public.cc @@ -555,36 +555,27 @@ Status impl::removeIsolatedCallback(secondary_app::Callback callback) return Status_Error; } -Status impl::secondaryAppDataExtract(feature_detector::Header &header, const uint8_t * data, const size_t len, const int64_t frameId) +Status impl::secondaryAppDataExtract(feature_detector::Header &header, const secondary_app::Header &orig) { - utility::BufferStreamReader stream(data, len); - wire::FeatureDetector featureDetector(stream, 1); //TODO + utility::BufferStreamReader stream(reinterpret_cast(orig.secondaryAppDataP), orig.secondaryAppDataLength); + wire::FeatureDetector featureDetector(stream, 1); //TODO Version check + utility::BufferStreamReader metaStream(reinterpret_cast(orig.secondaryAppMetadataP), orig.secondaryAppMetadataLength); - wire::FeatureDetectorMeta m; - if (findMetadata(&m, frameId) == Status_Ok) - { - fprintf(stderr, "%s Meta data extracted\n", __func__ ); - } - else - { - fprintf(stderr, "%s Error metadata extraction for frame %ld failed\n", __func__, frameId ); - return Status_Error; - } - + wire::FeatureDetectorMeta _meta(metaStream, 1); header.source = featureDetector.source; - header.frameId = m.frameId; - header.timeSeconds = m.timeSeconds; - header.timeNanoSeconds= m.timeNanoSeconds; - header.ptpNanoSeconds = m.ptpNanoSeconds; - header.octaveWidth = m.octaveWidth; - header.octaveHeight = m.octaveHeight; - header.numOctaves = m.numOctaves; - header.scaleFactor = m.scaleFactor; - header.motionStatus = m.motionStatus; - header.averageXMotion = m.averageXMotion; - header.averageYMotion = m.averageYMotion; + header.frameId = _meta.frameId; + header.timeSeconds = _meta.timeSeconds; + header.timeNanoSeconds= _meta.timeNanoSeconds; + header.ptpNanoSeconds = _meta.ptpNanoSeconds; + header.octaveWidth = _meta.octaveWidth; + header.octaveHeight = _meta.octaveHeight; + header.numOctaves = _meta.numOctaves; + header.scaleFactor = _meta.scaleFactor; + header.motionStatus = _meta.motionStatus; + header.averageXMotion = _meta.averageXMotion; + header.averageYMotion = _meta.averageYMotion; header.numFeatures = featureDetector.numFeatures; header.numDescriptors = featureDetector.numDescriptors; diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh b/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh index fc9b5364..4b434628 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh @@ -511,7 +511,7 @@ public: * @return A crl::multisense::Status indicating if the callback deregistration * succeeded or failed */ - virtual Status secondaryAppDataExtract(feature_detector::Header &h, const uint8_t * data, const size_t len, const int64_t frameId) = 0; + virtual Status secondaryAppDataExtract(feature_detector::Header &h, const secondary_app::Header &origh) = 0; /** * Reserve image or lidar data within a isolated callback so it is available diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh index 97322221..a8d84ce2 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh @@ -3102,6 +3102,10 @@ public: uint32_t secondaryAppDataLength; /** A pointer to the secondaryAppData data */ const void *secondaryAppDataP; + /** The metadata length */ + uint32_t secondaryAppMetadataLength; + /** The metadata generic payload*/ + const void *secondaryAppMetadataP; /** * Default Constructor diff --git a/source/LibMultiSense/include/MultiSense/details/channel.hh b/source/LibMultiSense/include/MultiSense/details/channel.hh index 088f2884..bc29688d 100644 --- a/source/LibMultiSense/include/MultiSense/details/channel.hh +++ b/source/LibMultiSense/include/MultiSense/details/channel.hh @@ -104,8 +104,6 @@ public: // // Public API - virtual Status findMetadata (wire::FeatureDetectorMeta * f, const int64_t & frameId); - virtual Status addIsolatedCallback (image::Callback callback, DataSource imageSourceMask, void *userDataP); @@ -142,7 +140,7 @@ public: virtual Status removeIsolatedCallback(secondary_app::Callback callback); // virtual Status removeIsolatedCallback(feature_detector::Callback callback); - virtual Status secondaryAppDataExtract(feature_detector::Header &h, const uint8_t * data, const size_t len, const int64_t frameId); + virtual Status secondaryAppDataExtract(feature_detector::Header &h, const secondary_app::Header &orig); virtual void* reserveCallbackBuffer (); virtual Status releaseCallbackBuffer (void *referenceP); @@ -310,7 +308,7 @@ private: static CRL_CONSTEXPR uint32_t RX_POOL_LARGE_BUFFER_SIZE = (10 * (1024 * 1024)); static CRL_CONSTEXPR uint32_t RX_POOL_LARGE_BUFFER_COUNT = 16; static CRL_CONSTEXPR uint32_t RX_POOL_SMALL_BUFFER_SIZE = (8 * (1024)); - static CRL_CONSTEXPR uint32_t RX_POOL_SMALL_BUFFER_COUNT = 128; + static CRL_CONSTEXPR uint32_t RX_POOL_SMALL_BUFFER_COUNT = 256; static CRL_CONSTEXPR uint32_t MAX_BUFFER_ALLOCATION_RETRIES = 5; static double DEFAULT_ACK_TIMEOUT () { return 0.5; } @@ -460,11 +458,6 @@ private: DepthCache m_imageMetaCache; - // - // A cache of feature detector meta data - - // DepthCache m_featureDetectorMetaCache; - // // A cache of secondary application meta data diff --git a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh index 7e13d70e..c145d79f 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/SecondaryAppMetaMessage.hh @@ -82,9 +82,18 @@ public: // // Constructors - SecondaryAppMetadata(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);}; + SecondaryAppMetadata(utility::BufferStreamReader&r, VersionType v) { + mRef = new utility::BufferStreamReader(r); + serialize(r, v); + }; + SecondaryAppMetadata() {}; + ~SecondaryAppMetadata() { + if (mRef) + delete mRef; + }; + // // Serialization routine @@ -107,6 +116,8 @@ public: } } + + utility::BufferStreamReader * mRef; }; #endif // !SENSORPOD_FIRMWARE diff --git a/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc b/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc index 3a312aeb..921849ed 100644 --- a/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc +++ b/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc @@ -351,7 +351,9 @@ void featureDetectorCallback(const secondary_app::Header& header, feature_detector::Header fHeader; UserData *userData = reinterpret_cast(userDataP); - Status s = userData->channelP->secondaryAppDataExtract(fHeader, reinterpret_cast(header.secondaryAppDataP), header.secondaryAppDataLength, header.frameId); + Status s = userData->channelP->secondaryAppDataExtract(fHeader, header); + void * buffer = userData->channelP->reserveCallbackBuffer(); + if (s != Status_Ok) { fprintf(stderr, "%s Error failed extraction\n", __func__ ); @@ -389,6 +391,7 @@ void featureDetectorCallback(const secondary_app::Header& header, std::cout << "timeSeconds: " << fHeader.timeSeconds << "\n"; std::cout << "timeNanoSeconds: " << fHeader.timeNanoSeconds << "\n"; std::cout << "ptpNanoSeconds: " << fHeader.ptpNanoSeconds << "\n"; + userData->channelP->releaseCallbackBuffer(buffer); } } // anonymous From ae3147a126a6eedc335415079bbadf043dfd218e Mon Sep 17 00:00:00 2001 From: psmithcrl Date: Wed, 20 Nov 2024 12:49:32 -0500 Subject: [PATCH 17/22] Fixed the Buffer Stream invalid length bug --- source/LibMultiSense/details/dispatch.cc | 29 +------------------ source/LibMultiSense/details/public.cc | 22 -------------- .../include/MultiSense/MultiSenseTypes.hh | 2 -- .../include/MultiSense/details/channel.hh | 13 +++++---- .../details/utility/BufferStream.hh | 2 +- .../FeatureDetectorUtility.cc | 12 ++++---- 6 files changed, 15 insertions(+), 65 deletions(-) diff --git a/source/LibMultiSense/details/dispatch.cc b/source/LibMultiSense/details/dispatch.cc index 746c3c08..ef1f52d4 100644 --- a/source/LibMultiSense/details/dispatch.cc +++ b/source/LibMultiSense/details/dispatch.cc @@ -91,12 +91,6 @@ #include "MultiSense/details/wire/SecondaryAppGetRegisteredAppsMessage.hh" #include "MultiSense/details/wire/SecondaryAppRegisteredAppsMessage.hh" -#include "MultiSense/details/wire/FeatureDetectorConfigMessage.hh" -#include "MultiSense/details/wire/FeatureDetectorGetConfigMessage.hh" -#include "MultiSense/details/wire/FeatureDetectorControlMessage.hh" -#include "MultiSense/details/wire/FeatureDetectorMessage.hh" -#include "MultiSense/details/wire/FeatureDetectorMetaMessage.hh" - #include #define __STDC_FORMAT_MACROS @@ -250,24 +244,6 @@ void impl::dispatchAprilTagDetections(apriltag::Header& header) m_channelStatistics.numDispatchedGroundSurfaceSpline += 1; } -// -// Publish a feature detection event - -// void impl::dispatchFeatureDetections(feature_detector::Header& header) -// { -// utility::ScopedLock lock(m_dispatchLock); -// -// std::list::const_iterator it; -// -// for(it = m_featureDetectorListeners.begin(); -// it != m_featureDetectorListeners.end(); -// ++ it) -// (*it)->dispatch(header); -// -// utility::ScopedLock statsLock(m_statisticsLock); -// m_channelStatistics.numDispatchedFeatureDetections++; -// } - // // Publish Secondary App Data @@ -636,7 +612,7 @@ void impl::dispatch(utility::BufferStreamWriter& buffer) header.source = SecondaryApp.source | ((uint64_t)SecondaryApp.sourceExtended << 32); header.timeSeconds = SecondaryApp.timeSeconds; header.timeMicroSeconds = SecondaryApp.timeMicroSeconds; - header.length = SecondaryApp.length; + header.secondaryAppDataLength = SecondaryApp.length; header.secondaryAppDataP = SecondaryApp.dataP; header.secondaryAppMetadataP = metaP->dataP; header.secondaryAppMetadataLength = metaP->dataLength; @@ -733,9 +709,6 @@ void impl::dispatch(utility::BufferStreamWriter& buffer) case MSG_ID(wire::PtpStatusResponse::ID): m_messages.store(wire::PtpStatusResponse(stream, version)); break; - // case MSG_ID(wire::FeatureDetectorConfig::ID): - // m_messages.store(wire::FeatureDetectorConfig(stream, version)); - // break; default: CRL_DEBUG("unknown message received: id=%d, version=%d\n", diff --git a/source/LibMultiSense/details/public.cc b/source/LibMultiSense/details/public.cc index 31b516f7..895bb679 100644 --- a/source/LibMultiSense/details/public.cc +++ b/source/LibMultiSense/details/public.cc @@ -290,27 +290,6 @@ Status impl::addIsolatedCallback(apriltag::Callback callback, return Status_Ok; } -// -// Adds a new feature detector listener -// -// Status impl::addIsolatedCallback(feature_detector::Callback callback, -// void *userDataP) -// { -// try { -// -// utility::ScopedLock lock(m_dispatchLock); -// m_featureDetectorListeners.push_back(new FeatureDetectorListener(callback, -// 0, -// userDataP, -// MAX_USER_FEATURE_DETECTOR_QUEUE_SIZE)); -// -// } catch (const std::exception& e) { -// CRL_DEBUG("exception: %s\n", e.what()); -// return Status_Exception; -// } -// return Status_Ok; -// } - // // Removes an image listener @@ -561,7 +540,6 @@ Status impl::secondaryAppDataExtract(feature_detector::Header &header, const sec wire::FeatureDetector featureDetector(stream, 1); //TODO Version check utility::BufferStreamReader metaStream(reinterpret_cast(orig.secondaryAppMetadataP), orig.secondaryAppMetadataLength); - wire::FeatureDetectorMeta _meta(metaStream, 1); header.source = featureDetector.source; diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh index a8d84ce2..4ae11257 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh @@ -3088,8 +3088,6 @@ public: /** DataSource corresponding to secondaryAppDataP*/ DataSource source; - /** length of the secondaryAppData */ - uint32_t length; /** Unique ID used to describe an secondaryAppData. FrameIds increase sequentally from the device */ int64_t frameId; /** The time seconds value corresponding to when the secondaryAppData was captured*/ diff --git a/source/LibMultiSense/include/MultiSense/details/channel.hh b/source/LibMultiSense/include/MultiSense/details/channel.hh index bc29688d..319fda1f 100644 --- a/source/LibMultiSense/include/MultiSense/details/channel.hh +++ b/source/LibMultiSense/include/MultiSense/details/channel.hh @@ -315,13 +315,12 @@ private: static CRL_CONSTEXPR uint32_t DEFAULT_ACK_ATTEMPTS = 5; static CRL_CONSTEXPR uint32_t IMAGE_META_CACHE_DEPTH = 4; static CRL_CONSTEXPR uint32_t SECONDARY_APP_META_CACHE_DEPTH = 4; - static CRL_CONSTEXPR uint32_t FEATURE_DETECTOR_META_CACHE_DEPTH = 4; static CRL_CONSTEXPR uint32_t UDP_TRACKER_CACHE_DEPTH = 4; static CRL_CONSTEXPR uint32_t TIME_SYNC_OFFSET_DECAY = 8; #if __cplusplus > 199711L static_assert(RX_POOL_LARGE_BUFFER_COUNT > IMAGE_META_CACHE_DEPTH, "Image metadata depth cache too large"); - static_assert(RX_POOL_LARGE_BUFFER_COUNT > FEATURE_DETECTOR_META_CACHE_DEPTH, "Feature detector metadata depth cache too large"); + static_assert(RX_POOL_LARGE_BUFFER_COUNT > SECONDARY_APP_META_CACHE_DEPTH, "Secondary Application metadata depth cache too large"); static_assert(RX_POOL_LARGE_BUFFER_COUNT > UDP_TRACKER_CACHE_DEPTH, "UDP depth cache too large"); static_assert(RX_POOL_SMALL_BUFFER_COUNT > UDP_TRACKER_CACHE_DEPTH, "UDP depth cache too large"); #endif @@ -336,11 +335,17 @@ private: static CRL_CONSTEXPR uint32_t MAX_USER_IMAGE_QUEUE_SIZE = 8; static CRL_CONSTEXPR uint32_t MAX_USER_LASER_QUEUE_SIZE = 8; static CRL_CONSTEXPR uint32_t MAX_USER_COMPRESSED_IMAGE_QUEUE_SIZE = 8; + static CRL_CONSTEXPR uint32_t MAX_USER_SECONDARY_APP_QUEUE_SIZE = 8; + static CRL_CONSTEXPR uint32_t MAX_USER_GROUND_SURFACE_QUEUE_SIZE = 8; + static CRL_CONSTEXPR uint32_t MAX_USER_APRILTAG_QUEUE_SIZE = 8; #if __cplusplus > 199711L static_assert(RX_POOL_LARGE_BUFFER_COUNT > MAX_USER_IMAGE_QUEUE_SIZE, "Image queue too large"); static_assert(RX_POOL_LARGE_BUFFER_COUNT > MAX_USER_LASER_QUEUE_SIZE, "Laser queue too large"); static_assert(RX_POOL_LARGE_BUFFER_COUNT > MAX_USER_COMPRESSED_IMAGE_QUEUE_SIZE, "Compressed image queue too large"); + static_assert(RX_POOL_LARGE_BUFFER_COUNT > MAX_USER_SECONDARY_APP_QUEUE_SIZE, "Secondary App queue too large"); + static_assert(RX_POOL_LARGE_BUFFER_COUNT > MAX_USER_GROUND_SURFACE_QUEUE_SIZE,"Ground Surface queue too large"); + static_assert(RX_POOL_LARGE_BUFFER_COUNT > MAX_USER_APRILTAG_QUEUE_SIZE, "April Tag queue too large"); #endif // @@ -349,10 +354,6 @@ private: static CRL_CONSTEXPR uint32_t MAX_USER_PPS_QUEUE_SIZE = 2; static CRL_CONSTEXPR uint32_t MAX_USER_IMU_QUEUE_SIZE = 64; - static CRL_CONSTEXPR uint32_t MAX_USER_GROUND_SURFACE_QUEUE_SIZE = 8; - static CRL_CONSTEXPR uint32_t MAX_USER_APRILTAG_QUEUE_SIZE = 8; - static CRL_CONSTEXPR uint32_t MAX_USER_FEATURE_DETECTOR_QUEUE_SIZE = 8; - static CRL_CONSTEXPR uint32_t MAX_USER_SECONDARY_APP_QUEUE_SIZE = 8; // diff --git a/source/LibMultiSense/include/MultiSense/details/utility/BufferStream.hh b/source/LibMultiSense/include/MultiSense/details/utility/BufferStream.hh index f7ff5251..04863a03 100644 --- a/source/LibMultiSense/include/MultiSense/details/utility/BufferStream.hh +++ b/source/LibMultiSense/include/MultiSense/details/utility/BufferStream.hh @@ -93,7 +93,7 @@ public: void seek(std::size_t idx) { if (idx > m_size) - CRL_EXCEPTION("invalid seek location %d, [0, %d] valid\n", + CRL_EXCEPTION("invalid seek location %lu, [0, %lu] valid\n", idx, m_size); m_tell = idx; }; diff --git a/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc b/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc index 921849ed..ab2a0264 100644 --- a/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc +++ b/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc @@ -348,15 +348,15 @@ void imageCallback(const image::Header& header, void featureDetectorCallback(const secondary_app::Header& header, void *userDataP) { - feature_detector::Header fHeader; UserData *userData = reinterpret_cast(userDataP); - Status s = userData->channelP->secondaryAppDataExtract(fHeader, header); void * buffer = userData->channelP->reserveCallbackBuffer(); + Status s = userData->channelP->secondaryAppDataExtract(fHeader, header); if (s != Status_Ok) { - fprintf(stderr, "%s Error failed extraction\n", __func__ ); + fprintf(stderr, "%s Error Secondary App Data extraction failed\n", __func__ ); + userData->channelP->releaseCallbackBuffer(buffer); return; } @@ -374,9 +374,9 @@ void featureDetectorCallback(const secondary_app::Header& header, { it->second.featureTime = std::chrono::system_clock::now(); std::cout << "Feature received after image " << fHeader.frameId - << " Delta: " - << std::chrono::duration_cast(it->second.featureTime - it->second.imageTime).count() - << "ms\n"; + << " Delta: " + << std::chrono::duration_cast(it->second.featureTime - it->second.imageTime).count() + << "ms\n"; userData->elapsedTime.erase(it); } } From 8fd2683bc95aef7bb9c9a355ec414ac5e2c846a8 Mon Sep 17 00:00:00 2001 From: psmithcrl Date: Fri, 22 Nov 2024 14:01:51 -0500 Subject: [PATCH 18/22] Refactored feature detector to make more generic --- source/LibMultiSense/details/channel.cc | 20 -- source/LibMultiSense/details/public.cc | 88 +----- .../include/MultiSense/MultiSenseChannel.hh | 64 +---- .../include/MultiSense/MultiSenseTypes.hh | 245 +--------------- .../include/MultiSense/details/channel.hh | 14 +- .../include/MultiSense/details/listeners.hh | 1 - .../wire/FeatureDetectorConfigMessage.hh | 102 ------- .../wire/FeatureDetectorControlMessage.hh | 101 ------- .../wire/FeatureDetectorGetConfigMessage.hh | 79 ------ .../MultiSense/details/wire/Protocol.hh | 25 +- .../FeatureDetectorUtility/CMakeLists.txt | 3 +- .../FeatureDetectorUtility.cc | 20 +- .../include}/FeatureDetectorMessage.hh | 19 +- .../include}/FeatureDetectorMetaMessage.hh | 46 ++- .../include/FeatureDetectorUtilities.hh | 266 ++++++++++++++++++ 15 files changed, 322 insertions(+), 771 deletions(-) delete mode 100644 source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorConfigMessage.hh delete mode 100644 source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorControlMessage.hh delete mode 100644 source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorGetConfigMessage.hh rename source/{LibMultiSense/include/MultiSense/details/wire => Utilities/FeatureDetectorUtility/include}/FeatureDetectorMessage.hh (88%) rename source/{LibMultiSense/include/MultiSense/details/wire => Utilities/FeatureDetectorUtility/include}/FeatureDetectorMetaMessage.hh (85%) create mode 100644 source/Utilities/FeatureDetectorUtility/include/FeatureDetectorUtilities.hh diff --git a/source/LibMultiSense/details/channel.cc b/source/LibMultiSense/details/channel.cc index 3f9184bc..634db66d 100644 --- a/source/LibMultiSense/details/channel.cc +++ b/source/LibMultiSense/details/channel.cc @@ -79,7 +79,6 @@ 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(), @@ -94,7 +93,6 @@ impl::impl(const std::string& address, const RemoteHeadChannel& cameraId, const m_imuListeners(), m_compressedImageListeners(), m_secondaryAppListeners(), - // m_featureDetectorListeners(), m_watch(), m_messages(), m_streamsEnabled(0), @@ -271,11 +269,6 @@ void impl::cleanup() its != m_secondaryAppListeners.end(); its ++) delete *its; - // std::list::const_iterator itf; - // for(itf = m_featureDetectorListeners.begin(); - // itf != m_featureDetectorListeners.end(); - // ++ itf) - // delete *itf; BufferPool::const_iterator it; for(it = m_rxLargeBufferPool.begin(); it != m_rxLargeBufferPool.end(); @@ -292,7 +285,6 @@ void impl::cleanup() m_imuListeners.clear(); m_secondaryAppListeners.clear(); m_compressedImageListeners.clear(); - // m_featureDetectorListeners.clear(); m_rxLargeBufferPool.clear(); m_rxSmallBufferPool.clear(); @@ -481,12 +473,6 @@ wire::SourceType impl::sourceApiToWire(DataSource mask) 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; @@ -532,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; diff --git a/source/LibMultiSense/details/public.cc b/source/LibMultiSense/details/public.cc index 895bb679..aeb1d826 100644 --- a/source/LibMultiSense/details/public.cc +++ b/source/LibMultiSense/details/public.cc @@ -106,12 +106,6 @@ #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" @@ -534,48 +528,6 @@ Status impl::removeIsolatedCallback(secondary_app::Callback callback) return Status_Error; } -Status impl::secondaryAppDataExtract(feature_detector::Header &header, const secondary_app::Header &orig) -{ - utility::BufferStreamReader stream(reinterpret_cast(orig.secondaryAppDataP), orig.secondaryAppDataLength); - wire::FeatureDetector featureDetector(stream, 1); //TODO Version check - - utility::BufferStreamReader metaStream(reinterpret_cast(orig.secondaryAppMetadataP), orig.secondaryAppMetadataLength); - wire::FeatureDetectorMeta _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; -} - - - - // // Reserve the current callback buffer being used in a dispatch thread @@ -1621,10 +1573,12 @@ Status impl::getSecondaryAppConfig(system::SecondaryAppConfig& config) // Currently several sensor messages are combined and presented // to the user as one. -Status impl::setSecondaryAppConfig(const system::SecondaryAppConfig& c) +Status impl::setSecondaryAppConfig(system::SecondaryAppConfig& c) { wire::SecondaryAppControl cmd; + c.serialize(); + if (c.dataLength >= 1024) { std::cerr << "Error: data length too large" << std::endl; @@ -1679,42 +1633,6 @@ Status impl::secondaryAppDeactivate(const std::string &_name) return waitAck(cmd); } -Status impl::getFeatureDetectorConfig (system::FeatureDetectorConfig & c) -{ - wire::SecondaryAppConfig f; - - Status status = waitData(wire::SecondaryAppGetConfig(), f); - if (Status_Ok != status) - return status; - - - try - { - wire::FeatureDetectorConfig instConfig(f); - c.setNumberOfFeatures(instConfig.configItems.numberOfFeatures); - c.setGrouping(instConfig.configItems.grouping); - c.setMotion(instConfig.configItems.motion); - } - catch (std::runtime_error e) - { - std::cerr << e.what() << std::endl; - return Status_Error; - } - - - return Status_Ok; -} -Status impl::setFeatureDetectorConfig (const system::FeatureDetectorConfig & c) -{ - wire::FeatureDetectorControl f; - - f.controlItems.numberOfFeatures = c.numberOfFeatures(); - f.controlItems.grouping = c.grouping(); - f.controlItems.motion = c.motion(); - - return waitAck(f); -} - // // Sets the device info diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh b/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh index 4b434628..f2922b09 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh @@ -376,23 +376,6 @@ public: virtual Status addIsolatedCallback(secondary_app::Callback callback, void *userDataP=NULL) = 0; - /** - * Add a user defined callback attached to the feature detector 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 - * Surface data to - * - * @param userDataP A pointer to arbitrary user data. - * - * @return A crl::multisense::Status indicating if the callback registration - * succeeded or failed - */ - // virtual Status addIsolatedCallback(feature_detector::Callback callback, - // void *userDataP=NULL) = 0; - /** * Unregister a user defined image::Callback. This stops the callback * from receiving image data. @@ -477,18 +460,6 @@ public: virtual Status removeIsolatedCallback(apriltag::Callback callback) = 0; - /** - * Unregister a user defined feature_detector::Callback. This stops the callback - * from receiving feature data - * - * @param callback The user defined feature_detector::Callback to unregister - * - * @return A crl::multisense::Status indicating if the callback deregistration - * succeeded or failed - */ - - // virtual Status removeIsolatedCallback(feature_detector::Callback callback) = 0; - /** * Unregister a user defined secondary_app::Callback. This stops the callback * from receiving ground surface data @@ -500,19 +471,6 @@ public: */ virtual Status removeIsolatedCallback(secondary_app::Callback callback) = 0; - /** - * Provides an interface to convert generic secondary app data to usable feature detector data - * - * @param h the application defined feature detector header - * @param data the generic payload from the secondary application - * @param len the length of the generic payload - * @param frameId the frameId for metadata lookup - * - * @return A crl::multisense::Status indicating if the callback deregistration - * succeeded or failed - */ - virtual Status secondaryAppDataExtract(feature_detector::Header &h, const secondary_app::Header &origh) = 0; - /** * Reserve image or lidar data within a isolated callback so it is available * after the callback returns. @@ -1111,26 +1069,6 @@ public: */ virtual Status setApriltagParams (const system::ApriltagParams& params) = 0; - /** - * Set the feature detector config associated with the MultiSense device - * - * @param params The feature detector parameters to send to the on-camera feature detector - * application - * - * @return A crl::multisense::Status indicating if the params were successfully set - */ - virtual Status setFeatureDetectorConfig (const system::FeatureDetectorConfig& params) = 0; - - /** - * Get the feature detector parameters associated with the MultiSense device - * - * @param params The feature detector parameters to send to the on-camera feature detector - * application - * - * @return A crl::multisense::Status indicating if the params were successfully set - */ - virtual Status getFeatureDetectorConfig (system::FeatureDetectorConfig& params) = 0; - /** * Get the secondary application config parameters associated with the MultiSense device * @@ -1148,7 +1086,7 @@ public: * * @return A crl::multisense::Status indicating if the params were successfully set */ - virtual Status setSecondaryAppConfig (const system::SecondaryAppConfig & c) = 0; + virtual Status setSecondaryAppConfig (system::SecondaryAppConfig & c) = 0; /** * Query the secondary application(s) registered with the MultiSense device diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh index 4ae11257..4df7c3f7 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,12 +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); // Same as Source Feature Left -static CRL_CONSTEXPR DataSource Source_Secondary_App_Data_1 = (1ull<<19); // Same as Source Feature Right -static CRL_CONSTEXPR DataSource Source_Secondary_App_Data_2 = (1ull<<32); // Same as Source Feature Aux -static CRL_CONSTEXPR DataSource Source_Secondary_App_Data_3 = (1ull<<33); // Same as Source Feature Rectified Left -static CRL_CONSTEXPR DataSource Source_Secondary_App_Data_4 = (1ull<<34); // Same as Source Feature Rectified Right -static CRL_CONSTEXPR DataSource Source_Secondary_App_Data_5 = (1ull<<35); // Same as Source Feature Rectified Aux +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 @@ -3024,58 +3018,6 @@ 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; - }; - - /** - * Function pointer for receiving callbacks for feature_detector data - */ - typedef void (*Callback)(const Header& header, - void *userDataP); - -} // namespace feature_detector - - namespace secondary_app { /** @@ -3990,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(7200); - * - * 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 @@ -4280,13 +4051,15 @@ public: { if (_length >= 1024) { - std::cerr << "Error: Secondary Application Config Length > 1023b" << std::endl; + 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 { diff --git a/source/LibMultiSense/include/MultiSense/details/channel.hh b/source/LibMultiSense/include/MultiSense/details/channel.hh index 319fda1f..84df166f 100644 --- a/source/LibMultiSense/include/MultiSense/details/channel.hh +++ b/source/LibMultiSense/include/MultiSense/details/channel.hh @@ -49,7 +49,6 @@ #include "MultiSense/details/wire/Protocol.hh" #include "MultiSense/details/wire/ImageMetaMessage.hh" #include "MultiSense/details/wire/SecondaryAppMetaMessage.hh" -#include "MultiSense/details/wire/FeatureDetectorMetaMessage.hh" #include "MultiSense/details/wire/StatusResponseMessage.hh" #include "MultiSense/details/wire/PtpStatusResponseMessage.hh" #include "MultiSense/details/wire/VersionResponseMessage.hh" @@ -127,9 +126,6 @@ public: virtual Status addIsolatedCallback (secondary_app::Callback callback, void *userDataP); - // virtual Status addIsolatedCallback (feature_detector::Callback callback, - // void *userDataP); - virtual Status removeIsolatedCallback(image::Callback callback); virtual Status removeIsolatedCallback(lidar::Callback callback); virtual Status removeIsolatedCallback(pps::Callback callback); @@ -138,9 +134,6 @@ public: virtual Status removeIsolatedCallback(ground_surface::Callback callback); virtual Status removeIsolatedCallback(apriltag::Callback callback); virtual Status removeIsolatedCallback(secondary_app::Callback callback); - // virtual Status removeIsolatedCallback(feature_detector::Callback callback); - - virtual Status secondaryAppDataExtract(feature_detector::Header &h, const secondary_app::Header &orig); virtual void* reserveCallbackBuffer (); virtual Status releaseCallbackBuffer (void *referenceP); @@ -233,14 +226,11 @@ public: virtual Status getLocalUdpPort (uint16_t& port); virtual Status getSecondaryAppConfig (system::SecondaryAppConfig & c); - virtual Status setSecondaryAppConfig (const 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 Status getFeatureDetectorConfig (system::FeatureDetectorConfig & c); - virtual Status setFeatureDetectorConfig (const system::FeatureDetectorConfig & c); - virtual system::ChannelStatistics getStats(); private: @@ -508,7 +498,6 @@ private: std::list m_groundSurfaceSplineListeners; std::list m_aprilTagDetectionListeners; std::list m_secondaryAppListeners; - // std::list m_featureDetectorListeners; // // A message signal interface @@ -599,7 +588,6 @@ private: void dispatchAprilTagDetections(apriltag::Header& header); void dispatchSecondaryApplication(utility::BufferStream& buffer, secondary_app::Header& header); - void dispatchFeatureDetections(feature_detector::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 36350ec7..4aa6c5e8 100644 --- a/source/LibMultiSense/include/MultiSense/details/listeners.hh +++ b/source/LibMultiSense/include/MultiSense/details/listeners.hh @@ -235,7 +235,6 @@ typedef Listener ImuListen typedef Listener CompressedImageListener; typedef Listener GroundSurfaceSplineListener; typedef Listener AprilTagDetectionListener; -typedef Listener FeatureDetectorListener; typedef Listener SecondaryAppListener; } // namespace details diff --git a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorConfigMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorConfigMessage.hh deleted file mode 100644 index fac4cab4..00000000 --- a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorConfigMessage.hh +++ /dev/null @@ -1,102 +0,0 @@ -/** - * @file LibMultiSense/FeatureDetectorConfigMessage.hh - * - * This message contains the current feature detector configuration. - * - * Copyright 2013-2024 - * Carnegie Robotics, LLC - * 4501 Hatfield Street, Pittsburgh, PA 15201 - * http://www.carnegiearobotics.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-25-01, patrick.smith@carnegierobotics.com, IRAD, Created file. - **/ - -#ifndef LibMultisense_FeatureDetectorConfigMessage -#define LibMultisense_FeatureDetectorConfigMessage - -#include "MultiSense/details/utility/Portability.hh" -#include "MultiSense/details/wire/Protocol.hh" -#include "MultiSense/details/wire/SecondaryAppConfigMessage.hh" - -namespace crl { -namespace multisense { -namespace details { -namespace wire { - -struct FeatureDetectorConfigItems { - - // - // The maximum number of features detected per image - uint32_t numberOfFeatures; - - // - // Enable/Disable feature grouping - bool grouping; - - // - // Enable motion detection - // Currently this functions as enable/disable but could be used to specify - // which octave motion detection is performed on. - // Current Octave: 3 - uint32_t motion; - -}; - -class FeatureDetectorConfig { -public: - static CRL_CONSTEXPR IdType ID = ID_DATA_FEATURE_DETECTOR_CONFIG; - static CRL_CONSTEXPR VersionType VERSION = 1; - - // - // Parameters representing the current camera configuration - - FeatureDetectorConfigItems configItems; - - SecondaryAppConfig configData; - - // - // Constructors - - FeatureDetectorConfig( SecondaryAppConfig & d ) { - - configData = d; - if (d.dataLength != sizeof(configItems)) - { - throw std::runtime_error("Error Invalid receipt length\n"); - } - - memcpy(&configItems, d.data, d.dataLength); - - }; - FeatureDetectorConfig() {}; - -}; - -}}}} // namespaces - -#endif diff --git a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorControlMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorControlMessage.hh deleted file mode 100644 index 5fb8d46c..00000000 --- a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorControlMessage.hh +++ /dev/null @@ -1,101 +0,0 @@ -/** - * @file LibMultiSense/FeatureDetectorControlMessage.hh - * - * This message contains the current feature detector configuration. - * - * Copyright 2013-2024 - * Carnegie Robotics, LLC - * 4501 Hatfield Street, Pittsburgh, PA 15201 - * http://www.carnegiearobotics.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-25-01, patrick.smith@carnegierobotics.com, IRAD, Created file. - **/ - -#ifndef LibMultisense_FeatureDetectorControlMessage -#define LibMultisense_FeatureDetectorControlMessage - -#include "MultiSense/details/utility/Portability.hh" -#include "MultiSense/details/wire/Protocol.hh" - -#include "MultiSense/details/wire/SecondaryAppControlMessage.hh" - -namespace crl { -namespace multisense { -namespace details { -namespace wire { - -struct FeatureDetectorControlItems { - - uint32_t numberOfFeatures; - - uint32_t grouping; - - uint32_t motion; - -} ; - -class FeatureDetectorControl: public SecondaryAppControl { -public: - static CRL_CONSTEXPR IdType ID = ID_CMD_FEATURE_DETECTOR_CONTROL; - static CRL_CONSTEXPR VersionType VERSION = 1; - - // - // Parameters representing the current camera configuration - FeatureDetectorControlItems controlItems; - - // - // Constructors - - FeatureDetectorControl(utility::BufferStreamReader&r, VersionType v) { - SecondaryAppControl::serialize(r,v); - memcpy(&controlItems, data, dataLength); - }; - FeatureDetectorControl() {}; - - // - // Serialization routine - - template - void serialize(Archive& message, - const VersionType version) - { - - (void) version; - - memset(data, 0, sizeof(data)); - dataLength = sizeof(FeatureDetectorControlItems); - memcpy(data, &controlItems, dataLength); - - SecondaryAppControl::serialize(message, version); - - } -}; - -}}}} // namespaces - -#endif diff --git a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorGetConfigMessage.hh b/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorGetConfigMessage.hh deleted file mode 100644 index d83c1a74..00000000 --- a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorGetConfigMessage.hh +++ /dev/null @@ -1,79 +0,0 @@ -/** - * @file LibMultiSense/FeatureDetectorGetConfigMessage.hh - * - * This message contains the request to get the current feature detector - * configuration. - * - * Copyright 2013-2024 - * Carnegie Robotics, LLC - * 4501 Hatfield Street, Pittsburgh, PA 15201 - * http://www.carnegiearobotics.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-25-01, patrick.smith@carnegierobotics.com, IRAD, Created file. - **/ - -#ifndef LibMultisense_FeatureDetectorGetConfigMessage -#define LibMultisense_FeatureDetectorGetConfigMessage - -#include "MultiSense/details/utility/Portability.hh" -#include "MultiSense/details/wire/Protocol.hh" - -namespace crl { -namespace multisense { -namespace details { -namespace wire { - -class FeatureDetectorGetConfig { -public: - static CRL_CONSTEXPR IdType ID = ID_CMD_FEATURE_DETECTOR_GET_CONFIG; - static CRL_CONSTEXPR VersionType VERSION = 1; - - // - // Parameters representing the current camera configuration - - // - // Constructors - - FeatureDetectorGetConfig(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);}; - FeatureDetectorGetConfig() {}; - - // - // Serialization routine - - template - void serialize(Archive& message, - const VersionType version) - { - (void) message; - (void) version; - } -}; - -}}}} // namespaces - -#endif diff --git a/source/LibMultiSense/include/MultiSense/details/wire/Protocol.hh b/source/LibMultiSense/include/MultiSense/details/wire/Protocol.hh index 4aef0441..0502f334 100644 --- a/source/LibMultiSense/include/MultiSense/details/wire/Protocol.hh +++ b/source/LibMultiSense/include/MultiSense/details/wire/Protocol.hh @@ -270,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); @@ -294,12 +288,12 @@ 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); // same as SOURCE_FEATURE_LEFT -static CRL_CONSTEXPR SourceType SOURCE_SECONDARY_APP_DATA_1 = (1ull<<19); // same as SOURCE_FEATURE_RIGHT -static CRL_CONSTEXPR SourceType SOURCE_SECONDARY_APP_DATA_2 = (1ull<<32); // same as SOURCE_FEATURE_AUX -static CRL_CONSTEXPR SourceType SOURCE_SECONDARY_APP_DATA_3 = (1ull<<33); // same as SOURCE_FEATURE_RECTIFIED_LEFT -static CRL_CONSTEXPR SourceType SOURCE_SECONDARY_APP_DATA_4 = (1ull<<34); // same as SOURCE_FEATURE_RECTIFIED_RIGHT -static CRL_CONSTEXPR SourceType SOURCE_SECONDARY_APP_DATA_5 = (1ull<<35); // same as SOURCE_FEATURE_RECTIFIED_AUX +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 | @@ -337,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/Utilities/FeatureDetectorUtility/CMakeLists.txt b/source/Utilities/FeatureDetectorUtility/CMakeLists.txt index 29b11181..1f38990b 100644 --- a/source/Utilities/FeatureDetectorUtility/CMakeLists.txt +++ b/source/Utilities/FeatureDetectorUtility/CMakeLists.txt @@ -25,7 +25,8 @@ if ((CMAKE_CXX_COMPILER_ID MATCHES MSVC AND # 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/FeatureDetectorUtility.cc b/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtility.cc index ab2a0264..635ca3e6 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 @@ -351,7 +353,7 @@ void featureDetectorCallback(const secondary_app::Header& header, feature_detector::Header fHeader; UserData *userData = reinterpret_cast(userDataP); void * buffer = userData->channelP->reserveCallbackBuffer(); - Status s = userData->channelP->secondaryAppDataExtract(fHeader, header); + Status s = feature_detector::secondaryAppDataExtract(fHeader, header); if (s != Status_Ok) { @@ -360,8 +362,8 @@ void featureDetectorCallback(const secondary_app::Header& header, return; } - if ((fHeader.source == Source_Feature_Left) - || (fHeader.source == Source_Feature_Rectified_Left)) { + 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()) { @@ -524,11 +526,11 @@ int main(int argc, fprintf(stderr, "%s got registered app: %s activated\n", __func__, s.apps[0].appName.c_str() ); - system::FeatureDetectorConfig c; + feature_detector::FeatureDetectorConfig c; c.setNumberOfFeatures(5000); c.setGrouping(1); c.setMotion(0); - status = channelP->setFeatureDetectorConfig(c); + status = channelP->setSecondaryAppConfig(c); if (Status_Ok != status) { std::cerr << "Error failed to set featureDetectorConfig apps\n"; @@ -537,9 +539,9 @@ int main(int argc, std::cout << "Successfully Configured Feature Detector\n"; - system::FeatureDetectorConfig fcfg; + feature_detector::FeatureDetectorConfig fcfg; - status = channelP->getFeatureDetectorConfig(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; @@ -557,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"; } @@ -615,7 +617,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; diff --git a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorMessage.hh b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorMessage.hh similarity index 88% rename from source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorMessage.hh rename to source/Utilities/FeatureDetectorUtility/include/FeatureDetectorMessage.hh index 97dd19ad..baced25e 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 @@ -58,7 +59,6 @@ public: uint8_t octave; uint8_t descriptor; -#ifndef SENSORPOD_FIRMWARE template void serialize(Archive& message, const VersionType version) @@ -71,7 +71,6 @@ public: message & octave; message & descriptor; } -#endif // !SENSORPOD_FIRMWARE }; class Descriptor { @@ -79,7 +78,6 @@ public: uint32_t d[8]; -#ifndef SENSORPOD_FIRMWARE template void serialize(Archive& message, const VersionType version) @@ -90,28 +88,18 @@ public: message & d[i]; } } -#endif // !SENSORPOD_FIRMWARE }; -class WIRE_HEADER_ATTRIBS_ FeatureDetectorHeader { +class FeatureDetectorHeader { public: - static CRL_CONSTEXPR IdType ID = ID_DATA_FEATURE_DETECTOR; static CRL_CONSTEXPR VersionType VERSION = 1; -#ifdef SENSORPOD_FIRMWARE - IdType id; - VersionType version; -#endif // SENSORPOD_FIRMWARE uint64_t source; int64_t frameId; uint16_t numFeatures; uint16_t numDescriptors; FeatureDetectorHeader() : -#ifdef SENSORPOD_FIRMWARE - id(ID), - version(VERSION), -#endif // SENSORPOD_FIRMWARE source(0), frameId(0), numFeatures(0), @@ -120,11 +108,9 @@ public: }; -#ifndef SENSORPOD_FIRMWARE 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; @@ -165,7 +151,6 @@ public: }; -#endif // !SENSORPOD_FIRMWARE }}}} // namespaces diff --git a/source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorMetaMessage.hh b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorMetaMessage.hh similarity index 85% rename from source/LibMultiSense/include/MultiSense/details/wire/FeatureDetectorMetaMessage.hh rename to source/Utilities/FeatureDetectorUtility/include/FeatureDetectorMetaMessage.hh index 652ac5a8..c2afc1a7 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 @@ -46,9 +47,8 @@ namespace multisense { namespace details { namespace wire { - class WIRE_HEADER_ATTRIBS_ FeatureDetectorMetaHeader { +class FeatureDetectorMetaHeader { public: - static CRL_CONSTEXPR IdType ID = ID_DATA_FEATURE_DETECTOR_META; static CRL_CONSTEXPR VersionType VERSION = 1; VersionType version; uint32_t length; @@ -67,28 +67,26 @@ namespace wire { uint16_t numFeatures; uint16_t numDescriptors; - 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) - {}; + 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 +}; class FeatureDetectorMeta : public FeatureDetectorMetaHeader { public: @@ -127,8 +125,6 @@ public: } }; -#endif // !SENSORPOD_FIRMWARE - }}}} // namespaces #endif diff --git a/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorUtilities.hh b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorUtilities.hh new file mode 100644 index 00000000..14e8c183 --- /dev/null +++ b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorUtilities.hh @@ -0,0 +1,266 @@ +/** + * @file LibMultiSense/FeatureDetectorMetaMessage.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-01-25, patrick.smith@carnegierobotics.com, IRAD, created file. + * 2024-22-11, patrick.smith@carnegierobotics.com, IRAD, Moved 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" + +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: + + struct FeatureDetectorConfigItems { + + // + // The maximum number of features detected per image + uint32_t numberOfFeatures; + + // + // Enable/Disable feature grouping + bool grouping; + + // + // Enable motion detection + // Currently this functions as enable/disable but could be used to specify + // which octave motion detection is performed on. + // Current Octave: 3 + uint32_t motion; + + } 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) +{ + + utility::BufferStreamReader stream(reinterpret_cast(orig.secondaryAppDataP), orig.secondaryAppDataLength); + wire::FeatureDetector featureDetector(stream, 1); //TODO Version check + + printf("[%s] f: %ld\n", __func__, featureDetector.frameId ); + + 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; +} + +} // namespace util + + +#endif /* end of include guard: __FEATURE_DETECTOR_UTILITIES_H__ */ From 99c28e9989cffc0c7d5376222f99e1dd39512cea Mon Sep 17 00:00:00 2001 From: psmithcrl Date: Fri, 22 Nov 2024 16:45:28 -0500 Subject: [PATCH 19/22] Moved definition to cc file for external linkage --- .../FeatureDetectorUtility/CMakeLists.txt | 2 +- .../FeatureDetectorUtilities.cc | 83 +++++++++++++++++++ .../include/FeatureDetectorUtilities.hh | 46 +--------- 3 files changed, 87 insertions(+), 44 deletions(-) create mode 100644 source/Utilities/FeatureDetectorUtility/FeatureDetectorUtilities.cc diff --git a/source/Utilities/FeatureDetectorUtility/CMakeLists.txt b/source/Utilities/FeatureDetectorUtility/CMakeLists.txt index 1f38990b..be3990c2 100644 --- a/source/Utilities/FeatureDetectorUtility/CMakeLists.txt +++ b/source/Utilities/FeatureDetectorUtility/CMakeLists.txt @@ -18,7 +18,7 @@ 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. diff --git a/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtilities.cc b/source/Utilities/FeatureDetectorUtility/FeatureDetectorUtilities.cc new file mode 100644 index 00000000..df870820 --- /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/include/FeatureDetectorUtilities.hh b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorUtilities.hh index 14e8c183..f3d889f3 100644 --- a/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorUtilities.hh +++ b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorUtilities.hh @@ -1,7 +1,7 @@ /** - * @file LibMultiSense/FeatureDetectorMetaMessage.hh + * @file LibMultiSense/FeatureDetectorMeta.hh * - * Copyright 2013-2022 + * Copyright 2013-2024 * Carnegie Robotics, LLC * 4501 Hatfield Street, Pittsburgh, PA 15201 * http://www.carnegierobotics.com @@ -31,7 +31,6 @@ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * * 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. **/ @@ -218,47 +217,8 @@ public: * @return A crl::multisense::Status indicating if the callback deregistration * succeeded or failed */ -Status secondaryAppDataExtract(feature_detector::Header &header, const secondary_app::Header &orig) -{ +Status secondaryAppDataExtract(feature_detector::Header &header, const secondary_app::Header &orig); - utility::BufferStreamReader stream(reinterpret_cast(orig.secondaryAppDataP), orig.secondaryAppDataLength); - wire::FeatureDetector featureDetector(stream, 1); //TODO Version check - - printf("[%s] f: %ld\n", __func__, featureDetector.frameId ); - - 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; -} } // namespace util From 20ea376396e56d71f116f02bd67d037efd17a58a Mon Sep 17 00:00:00 2001 From: psmithcrl Date: Fri, 22 Nov 2024 17:44:03 -0500 Subject: [PATCH 20/22] Separated the config so I can re-use it in other places --- .../include/FeatureDetectorConfig.hh | 88 +++++++++++++++++++ .../include/FeatureDetectorMessage.hh | 10 ++- .../include/FeatureDetectorUtilities.hh | 24 +---- 3 files changed, 97 insertions(+), 25 deletions(-) create mode 100644 source/Utilities/FeatureDetectorUtility/include/FeatureDetectorConfig.hh diff --git a/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorConfig.hh b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorConfig.hh new file mode 100644 index 00000000..2df90178 --- /dev/null +++ b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorConfig.hh @@ -0,0 +1,88 @@ +/** + * @file LibMultiSense/FeatureDetectorMessage.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_CONFIG_H__ +#define __FEATURE_DETECTOR_CONFIG_H__ + +#include "MultiSense/details/utility/Portability.hh" + +namespace crl { +namespace multisense { +namespace details { +namespace wire { + +struct FeatureDetectorConfigParams { + + + static CRL_CONSTEXPR VersionType VERSION = 1; + + // + // The message version + VersionType version; + + // + // The maximum number of features detected per image + uint32_t numberOfFeatures; + + // + // Enable/Disable feature grouping + bool grouping; + + // + // Enable motion detection + // Currently this functions as enable/disable but could be used to specify + // which octave motion detection is performed on. + // Current Octave: 3 + uint32_t motion; + + FeatureDetectorConfigParams ( ) : + version(VERSION), + numberOfFeatures(1500), + grouping(true), + motion(1) + { + + } + +}; + +} +} +} +} + +#endif /* end of include guard: __FEATURE_DETECTOR_CONFIG_H__ */ diff --git a/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorMessage.hh b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorMessage.hh index baced25e..60d36eb2 100644 --- a/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorMessage.hh +++ b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorMessage.hh @@ -93,18 +93,19 @@ public: class FeatureDetectorHeader { public: static CRL_CONSTEXPR VersionType VERSION = 1; - + VersionType version; uint64_t source; int64_t frameId; uint16_t numFeatures; uint16_t numDescriptors; FeatureDetectorHeader() : + version(VERSION), source(0), frameId(0), numFeatures(0), numDescriptors(0) - {}; + {}; }; @@ -126,10 +127,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; diff --git a/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorUtilities.hh b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorUtilities.hh index f3d889f3..6c56fe74 100644 --- a/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorUtilities.hh +++ b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorUtilities.hh @@ -31,14 +31,12 @@ * 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. + * 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" @@ -47,6 +45,7 @@ #include "FeatureDetectorMessage.hh" #include "FeatureDetectorMetaMessage.hh" +#include "FeatureDetectorConfig.hh" using namespace crl::multisense; using namespace crl::multisense::details; @@ -70,24 +69,7 @@ class MULTISENSE_API FeatureDetectorConfig: public crl::multisense::system::Seco private: - struct FeatureDetectorConfigItems { - - // - // The maximum number of features detected per image - uint32_t numberOfFeatures; - - // - // Enable/Disable feature grouping - bool grouping; - - // - // Enable motion detection - // Currently this functions as enable/disable but could be used to specify - // which octave motion detection is performed on. - // Current Octave: 3 - uint32_t motion; - - } mConfigItems; + wire::FeatureDetectorConfigParams mConfigItems; public: /** From 044debe7d00dab4bf335e2ec1bb689dc6da099ee Mon Sep 17 00:00:00 2001 From: psmithcrl Date: Mon, 25 Nov 2024 12:25:49 -0500 Subject: [PATCH 21/22] Missing include for external build --- .../FeatureDetectorUtility/include/FeatureDetectorConfig.hh | 1 + 1 file changed, 1 insertion(+) diff --git a/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorConfig.hh b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorConfig.hh index 2df90178..198de67a 100644 --- a/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorConfig.hh +++ b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorConfig.hh @@ -39,6 +39,7 @@ #define __FEATURE_DETECTOR_CONFIG_H__ #include "MultiSense/details/utility/Portability.hh" +#include "MultiSense/MultiSenseTypes.hh" namespace crl { namespace multisense { From 87576699c790b0368df7dc058f0cae027540a31c Mon Sep 17 00:00:00 2001 From: psmithcrl Date: Mon, 25 Nov 2024 14:59:26 -0500 Subject: [PATCH 22/22] Explicit structure packing of Feature --- .../include/FeatureDetectorMessage.hh | 40 +++++-------------- .../include/FeatureDetectorMetaMessage.hh | 9 ++++- 2 files changed, 16 insertions(+), 33 deletions(-) diff --git a/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorMessage.hh b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorMessage.hh index 60d36eb2..e0167b07 100644 --- a/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorMessage.hh +++ b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorMessage.hh @@ -43,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; @@ -59,41 +60,18 @@ public: uint8_t octave; uint8_t descriptor; - template - void serialize(Archive& message, - const VersionType version) - { - (void) version; - message & x; - message & y; - message & angle; - message & resp; - message & octave; - message & descriptor; - } }; -class Descriptor { +class __attribute__((__packed__)) Descriptor { public: uint32_t d[8]; - - template - void serialize(Archive& message, - const VersionType version) - { - (void) version; - for (size_t i = 0; i < 8; i++) - { - message & d[i]; - } - } }; -class FeatureDetectorHeader { +class WIRE_HEADER_ATTRIBS_ FeatureDetectorHeader { public: - static CRL_CONSTEXPR VersionType VERSION = 1; - VersionType version; + static CRL_CONSTEXPR VersionType VERSION = 1; + wire::VersionType version; uint64_t source; int64_t frameId; uint16_t numFeatures; @@ -109,13 +87,11 @@ public: }; +#ifndef SENSORPOD_FIRMWARE class FeatureDetector : public FeatureDetectorHeader { public: 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; // @@ -153,6 +129,8 @@ public: }; +#endif // !SENSORPOD_FIRMWARE + }}}} // namespaces diff --git a/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorMetaMessage.hh b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorMetaMessage.hh index c2afc1a7..77e55518 100644 --- a/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorMetaMessage.hh +++ b/source/Utilities/FeatureDetectorUtility/include/FeatureDetectorMetaMessage.hh @@ -41,16 +41,17 @@ #include #include "MultiSense/details/utility/Portability.hh" +#include "MultiSense/details/wire/Protocol.hh" namespace crl { namespace multisense { namespace details { namespace wire { -class FeatureDetectorMetaHeader { +class WIRE_HEADER_ATTRIBS_ FeatureDetectorMetaHeader { public: static CRL_CONSTEXPR VersionType VERSION = 1; - VersionType version; + wire::VersionType version; uint32_t length; uint32_t source; int64_t frameId; @@ -88,6 +89,8 @@ class FeatureDetectorMetaHeader { }; +#ifndef SENSORPOD_FIRMWARE + class FeatureDetectorMeta : public FeatureDetectorMetaHeader { public: @@ -125,6 +128,8 @@ public: } }; +#endif // !SENSORPOD_FIRMWARE + }}}} // namespaces #endif