Add multi-channel synchronizer

CMakeLists.txt

@@ -13,7 +13,7 @@ if(WIN32 AND DEFINED ENV{VCPKG_ROOT})
     set(CMAKE_TOOLCHAIN_FILE "$ENV{VCPKG_ROOT}/scripts/buildsystems/vcpkg.cmake" CACHE STRING "vcpkg toolchain file")
 endif()
 
-set(version "7.2.0")
+set(version "7.4.0")
 set(CPACK_PACKAGE_VERSION ${version})
 
 project(LibMultiSense

python/bindings.cc

@@ -44,6 +44,7 @@
 #include <pybind11/stl.h>
 
 #include <MultiSense/MultiSenseChannel.hh>
+#include <MultiSense/MultiSenseMultiChannel.hh>
 #include <MultiSense/MultiSenseUtilities.hh>
 
 #ifdef BUILD_JSON
@@ -712,6 +713,37 @@ PYBIND11_MODULE(_libmultisense, m) {
         .def("get_system_status", &multisense::Channel::get_system_status, py::call_guard<py::gil_scoped_release>())
         .def("set_network_config", &multisense::Channel::set_network_config, py::call_guard<py::gil_scoped_release>());
 
+    // MultiChannelSynchronizer
+    m.def("frames_synchronized", &multisense::frames_synchronized, py::call_guard<py::gil_scoped_release>());
+
+    py::class_<multisense::MultiChannelSynchronizer>(m, "MultiChannelSynchronizer")
+        .def("__enter__", [](multisense::MultiChannelSynchronizer &self) -> multisense::MultiChannelSynchronizer &
+        {
+            return self;
+        })
+        .def("__exit__", [](multisense::MultiChannelSynchronizer &, py::object, py::object, py::object)
+        {
+            return false;
+        })
+        .def(py::init([](py::iterable channels, const std::chrono::nanoseconds &tolerance, py::ssize_t max_queue_size) {
+                std::vector<multisense::Channel*> ptrs;
+                ptrs.reserve(py::len(channels));
+
+                for (py::handle obj : channels) {
+                    ptrs.emplace_back(obj.cast<multisense::Channel*>());
+                }
+
+                return new multisense::MultiChannelSynchronizer(std::move(ptrs), tolerance, max_queue_size);
+            }),
+            py::arg("channels"),
+            py::arg("tolerance"),
+            py::arg("max_queue_size") = 0
+        )
+        .def("channel", &multisense::MultiChannelSynchronizer::channel, py::call_guard<py::gil_scoped_release>())
+        .def("get_synchronized_frame", py::overload_cast<>( &multisense::MultiChannelSynchronizer::get_synchronized_frame), py::call_guard<py::gil_scoped_release>())
+        .def("get_synchronized_frame", py::overload_cast< const std::optional<std::chrono::nanoseconds>&>( &multisense::MultiChannelSynchronizer::get_synchronized_frame),
+            py::arg("timeout") = std::nullopt, py::call_guard<py::gil_scoped_release>());
+
     // Utilities
     py::class_<multisense::QMatrix>(m, "QMatrix")
         .def(py::init<const multisense::CameraCalibration &, const multisense::CameraCalibration &>())

source/LibMultiSense/CMakeLists.txt

@@ -7,6 +7,7 @@ if (BUILD_JSON_SERIALIZATION)
 endif()
 
 set(DETAILS_SRC details/factory.cc
+                details/multi_channel.cc
                 details/utilities.cc
                 details/legacy/calibration.cc
                 details/legacy/channel.cc

source/LibMultiSense/details/multi_channel.cc

@@ -0,0 +1,124 @@
+/**
+ * @file multi_channel.cc
+ *
+ * Copyright 2013-2025
+ * 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):
+ *   2025-12-02, malvarado@carnegierobotics.com, IRAD, Created file.
+ **/
+
+#include <algorithm>
+
+#include "MultiSense/MultiSenseMultiChannel.hh"
+
+namespace multisense {
+
+bool frames_synchronized(const std::vector<ImageFrame> &frames, const std::chrono::nanoseconds &tolerance)
+{
+    using namespace std::chrono_literals;
+
+    if (frames.size() <= 1)
+    {
+        return true;
+    }
+
+    const auto [min, max] = std::minmax_element(std::begin(frames), std::end(frames),
+                                                [](const auto &lhs, const auto &rhs)
+                                                {
+                                                    return lhs.ptp_frame_time < rhs.ptp_frame_time;
+                                                });
+
+    return min->ptp_frame_time.time_since_epoch() != 0ns && std::chrono::abs(max->ptp_frame_time - min->ptp_frame_time) < tolerance;
+
+}
+
+
+void MultiChannelSynchronizer::add_user_callbacks()
+{
+    for(size_t i = 0 ; i < m_channels.size() ; ++i)
+    {
+        m_channels[i]->add_image_frame_callback([i, this](auto frame)
+                                                {
+                                                    std::lock_guard<std::mutex> lock(m_frame_mutex);
+                                                    m_active_frames[i] = std::move(frame);
+
+                                                    if (!m_ready_frames.empty())
+                                                    {
+                                                        m_frame_cv.notify_all();
+                                                    }
+
+                                                    if (frames_synchronized(m_active_frames, m_tolerance))
+                                                    {
+                                                        if (m_max_queue_size > 0 &&
+                                                            m_ready_frames.size() >= m_max_queue_size)
+                                                        {
+                                                            m_ready_frames.pop_front();
+                                                        }
+
+                                                        m_ready_frames.emplace_back(m_active_frames);
+                                                        for (auto &active_frame : m_active_frames)
+                                                        {
+                                                            active_frame = ImageFrame{};
+                                                        }
+
+                                                        m_frame_cv.notify_all();
+                                                    }
+                                                });
+    }
+}
+
+std::optional<std::vector<ImageFrame>> MultiChannelSynchronizer::get_synchronized_frame(const std::optional<std::chrono::nanoseconds> &timeout)
+{
+    std::unique_lock<std::mutex> lock(m_frame_mutex);
+    const auto frames_ready = [this]() { return !m_ready_frames.empty(); };
+
+    if (timeout)
+    {
+        if (!m_frame_cv.wait_for(lock, timeout.value(), frames_ready))
+        {
+            return std::nullopt;
+        }
+    }
+    else
+    {
+        m_frame_cv.wait(lock, frames_ready);
+    }
+
+    if (m_ready_frames.empty())
+    {
+        return std::nullopt;
+    }
+
+    auto output_frames = std::move(m_ready_frames.front());
+    m_ready_frames.pop_front();
+    return output_frames;
+}
+
+}

source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh

@@ -0,0 +1,193 @@
+/**
+ * @file MultiSenseMultiChannel.hh
+ *
+ * Copyright 2013-2025
+ * 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):
+ *   2025-12-02, malvarado@carnegierobotics.com, IRAD, Created file.
+ **/
+
+#pragma once
+
+#include <condition_variable>
+#include <deque>
+#include <mutex>
+
+#include "MultiSenseChannel.hh"
+
+namespace multisense {
+
+///
+/// @brief Free function which determines if a collection of frames are synchronized within a given tolerance
+///
+/// @param frames Frames to check for synchronization
+/// @param tolerance The max time difference between any frames for them to be considered valid
+///
+/// @return Return true if the frames are synchronized
+///
+MULTISENSE_API bool frames_synchronized(const std::vector<ImageFrame> &frames, const std::chrono::nanoseconds &tolerance);
+
+///
+/// @brief Helper class which provides a interface to synchronize data across multiple channels.
+///
+class MULTISENSE_API MultiChannelSynchronizer {
+public:
+
+   ///
+   /// @brief Construct a synchronizer owning the underlying channels
+   ///
+   /// @param channels The channels to synchronize
+   /// @param tolerance The max time difference for a set of images to be considered synchronized
+   /// @param max_queue_size The max number of synchronized frames to have queued for dispatch
+   ///
+   explicit MultiChannelSynchronizer(std::vector<std::unique_ptr<Channel>> channels,
+                                     const std::chrono::nanoseconds &tolerance,
+                                     size_t max_queue_size = 0):
+        m_owned_channels(std::move(channels)),
+        m_tolerance(tolerance),
+        m_max_queue_size(max_queue_size)
+    {
+        for (auto &channel : m_owned_channels)
+        {
+            const auto config = channel->get_config();
+
+            if (!config.time_config || !config.time_config->ptp_enabled)
+            {
+                throw std::runtime_error("Creating a MultiChannelSynchronizer with PTP disabled");
+            }
+
+            m_channels.push_back(channel.get());
+        }
+
+        m_active_frames.resize(m_channels.size());
+        add_user_callbacks();
+    }
+
+   ///
+   /// @brief Construct a synchronizer without owning the underlying channels
+   ///
+   /// @param channels The channels to synchronize
+   /// @param tolerance The max time difference for a set of images to be considered synchronized
+   /// @param max_queue_size The max number of synchronized frames to have queued for dispatch
+   ///
+   explicit MultiChannelSynchronizer(std::vector<Channel*> channels,
+                                     const std::chrono::nanoseconds &tolerance,
+                                     size_t max_queue_size = 0):
+        m_channels(std::move(channels)),
+        m_tolerance(tolerance),
+        m_max_queue_size(max_queue_size)
+    {
+        m_active_frames.resize(m_channels.size());
+        add_user_callbacks();
+    }
+
+    ~MultiChannelSynchronizer() = default;
+
+    ///
+    /// @brief Access a channel by index
+    ///
+    Channel& channel(size_t index)
+    {
+        if (index >= m_channels.size())
+        {
+            throw std::runtime_error("Invalid multi-channel access");
+        }
+
+        return *m_channels.at(index);
+    }
+
+    ///
+    /// @brief Get a collection synchronized frames from the input channels with no timeout on waiting
+    ///
+    ///  @return Return a collection of synchronized frames
+    ///
+    std::optional<std::vector<ImageFrame>> get_synchronized_frame()
+    {
+        return get_synchronized_frame(std::nullopt);
+    }
+
+    ///
+    /// @brief Get a collection synchronized frames from the input channels and return if we have not recieved
+    ///        a collection of frames before the input timeout
+    ///
+    ///  @param timeout The ammount of time to wait for a synchronized frame
+    ///  @return Return a collection of synchronized frames
+    ///
+    std::optional<std::vector<ImageFrame>> get_synchronized_frame(const std::optional<std::chrono::nanoseconds> &timeout);
+
+private:
+
+    ///
+    /// @brief Helper to add user callbacks to the input channels
+    ///
+    void add_user_callbacks();
+
+    ///
+    /// @brief The collection of channels raw channels to synchronize
+    ///
+    std::vector<Channel*> m_channels{};
+
+    ///
+    /// @brief A collection of owned channels if the user would like the synchronizer to own the channel memory
+    ///
+    std::vector<std::unique_ptr<Channel>> m_owned_channels{};
+
+    ///
+    /// @brief A collection of active frames which may be dispatched to the user
+    ///
+    std::vector<ImageFrame> m_active_frames{};
+
+    ///
+    /// @brief The max time tolerance between image frames for them to be considered equal
+    ///
+    std::chrono::nanoseconds m_tolerance{};
+
+    ///
+    /// @brief Maximum number of synchronized frame groups that will be queued (0 = unlimited)
+    ///
+    size_t m_max_queue_size = 0;
+
+    ///
+    /// @brief Mutex to notify the user a collection of synchronized frames is ready
+    ///
+    std::mutex m_frame_mutex;
+
+    ///
+    /// @brief Condition variable to notify the user a collection of synchronized frames is ready
+    ///
+    std::condition_variable m_frame_cv;
+
+    ///
+    /// @brief Queue of synchronized frames ready for the user to consume
+    ///
+    std::deque<std::vector<ImageFrame>> m_ready_frames{};
+};
+
+}

source/LibMultiSense/test/CMakeLists.txt

@@ -5,6 +5,7 @@ set(TEST_NAMES
     configuration_test
     info_test
     message_test
+    multi_channel_test
     multisense_utilities_test
     status_test
     storage_test