Added secondary application compiles, untested

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<SecondaryAppListener*>::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();
@@ -470,6 +477,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 +517,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;
 }

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/SecondaryAppDataMessage.hh"
+#include "MultiSense/details/wire/SecondaryAppControlMessage.hh"
+#include "MultiSense/details/wire/SecondaryAppConfigMessage.hh"
 
 #include <limits>
 
@@ -210,6 +213,22 @@ void impl::dispatchAprilTagDetections(apriltag::Header& header)
         (*it)->dispatch(header);
 }
 
+//
+// Publish Secondary App Data
+
+void impl::dispatchSecondaryApplication(utility::BufferStream& buffer,
+                                        secondary_app::Header& header)
+{
+    utility::ScopedLock lock(m_dispatchLock);
+
+    std::list<SecondaryAppListener*>::const_iterator it;
+
+    for(it  = m_secondaryAppListeners.begin();
+        it != m_secondaryAppListeners.end();
+        it ++)
+        (*it)->dispatch(buffer, header);
+}
+
 
 //
 // Dispatch incoming messages
@@ -542,6 +561,22 @@ void impl::dispatch(utility::BufferStreamWriter& buffer)
         dispatchAprilTagDetections(header);
         break;
     }
+    case MSG_ID(wire::SecondaryAppData::ID):
+    {
+        wire::SecondaryAppData SecondaryApp(stream, version);
+
+        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.secondaryAppDataP = SecondaryApp.dataP;
+        dispatchSecondaryApplication(buffer, header);
+        break;
+    }
     case MSG_ID(wire::Ack::ID):
         break; // handle below
     case MSG_ID(wire::CamConfig::ID):
@@ -610,6 +645,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",

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/SecondaryAppDataMessage.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<SecondaryAppListener*>::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
 

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.

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,49 @@ 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;
+    /** 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*/
+    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) {};
+
+};
+    /**
+     * Function pointer for receiving callbacks for apriltag data
+     */
+    typedef void (*Callback)(const Header& header,
+                             void         *userDataP);
+} // namespace secondary_app
+
 namespace system {
 
 /**
@@ -3850,6 +3894,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