From 9dd5f4a23ff0ebe2f20c5ac6292eddc9db7c20a0 Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Tue, 2 Dec 2025 16:48:52 -0500 Subject: [PATCH 01/16] Initial commit for multi-channel support --- source/LibMultiSense/CMakeLists.txt | 1 + source/LibMultiSense/details/multi_channel.cc | 92 +++++++++ .../MultiSense/MultiSenseMultiChannel.hh | 110 +++++++++++ source/Utilities/LibMultiSense/CMakeLists.txt | 1 + .../LibMultiSense/MultiChannel/CMakeLists.txt | 6 + .../MultiChannel/MultiChannelUtility.cc | 178 ++++++++++++++++++ 6 files changed, 388 insertions(+) create mode 100644 source/LibMultiSense/details/multi_channel.cc create mode 100644 source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh create mode 100644 source/Utilities/LibMultiSense/MultiChannel/CMakeLists.txt create mode 100644 source/Utilities/LibMultiSense/MultiChannel/MultiChannelUtility.cc diff --git a/source/LibMultiSense/CMakeLists.txt b/source/LibMultiSense/CMakeLists.txt index 5dc35fc2..52d93345 100644 --- a/source/LibMultiSense/CMakeLists.txt +++ b/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 diff --git a/source/LibMultiSense/details/multi_channel.cc b/source/LibMultiSense/details/multi_channel.cc new file mode 100644 index 00000000..eb7b2aae --- /dev/null +++ b/source/LibMultiSense/details/multi_channel.cc @@ -0,0 +1,92 @@ +/** + * @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 "MultiSense/MultiSenseMultiChannel.hh" + +namespace multisense { + +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 lock(m_frame_mutex); + m_active_frames[i] = frame; + + if (frames_valid(m_tolerance)) + { + m_frame_cv.notify_all(); + } + }); + } +} + +bool MultiChannelSynchronizer::frames_valid(const std::chrono::nanoseconds &tolerance) +{ + using namespace std::chrono_literals; + + const auto reference_time = m_active_frames.front().frame_time; + + return std::all_of(std::begin(m_active_frames), std::end(m_active_frames), + [reference_time, tolerance](const auto &frame) + { + return reference_time.time_since_epoch() != 0ns && (reference_time - frame.frame_time) < tolerance; + }); +} + +std::optional> MultiChannelSynchronizer::get_synchronized_frame(const std::optional &timeout) +{ + std::unique_lock lock(m_frame_mutex); + if (timeout) + { + if (std::cv_status::timeout == m_frame_cv.wait_for(lock, timeout.value())) + { + return std::nullopt; + } + } + else + { + m_frame_cv.wait(lock); + } + + auto output_frames = m_active_frames; + m_active_frames.clear(); + m_active_frames.resize(m_channels.size()); + return output_frames; +} + +} diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh b/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh new file mode 100644 index 00000000..38fda41a --- /dev/null +++ b/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh @@ -0,0 +1,110 @@ +/** + * @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 +#include + +#include "MultiSenseChannel.hh" + +namespace multisense { + +class MULTISENSE_API MultiChannelSynchronizer { +public: + + explicit MultiChannelSynchronizer(std::vector> channels, const std::chrono::nanoseconds &tolerance): + m_channels(std::move(channels)), + m_active_frames(channels.size()), + m_tolerance(tolerance) + { + add_user_callbacks(); + } + + template + explicit MultiChannelSynchronizer(ChannelT&&... channels, const std::chrono::nanoseconds &tolerance): + m_tolerance(tolerance) + { + m_channels.reserve(sizeof...(channels)); + m_active_frames.resize(sizeof...(channels)); + (m_channels.emplace_back(std::forward(channels)), ...); + + add_user_callbacks(); + } + + ~MultiChannelSynchronizer() = default; + + Channel& channel(size_t index) + { + if (index > m_channels.size()) + { + throw std::runtime_error("Invalid multi-channel access"); + } + + return *m_channels.at(index); + } + + std::optional> get_synchronized_frame() + { + return get_synchronized_frame(std::nullopt); + } + + std::optional> get_synchronized_frame(const std::optional &timeout); + +private: + + void add_user_callbacks(); + + bool frames_valid(const std::chrono::nanoseconds &tolerance); + + /// + /// @brief The collection of channels to synchronize + /// + std::vector> m_channels{}; + + /// + /// @brief A collection of active frames which may be dispatched to the user + /// + std::vector m_active_frames{}; + + std::chrono::nanoseconds m_tolerance{}; + + + std::mutex m_frame_mutex; + std::condition_variable m_frame_cv; +}; + +} diff --git a/source/Utilities/LibMultiSense/CMakeLists.txt b/source/Utilities/LibMultiSense/CMakeLists.txt index 3d21f9be..d677df55 100644 --- a/source/Utilities/LibMultiSense/CMakeLists.txt +++ b/source/Utilities/LibMultiSense/CMakeLists.txt @@ -29,6 +29,7 @@ endif () add_subdirectory(ChangeIpUtility) add_subdirectory(DeviceInfoUtility) add_subdirectory(ImageCalUtility) +add_subdirectory(MultiChannel) add_subdirectory(PointCloudUtility) add_subdirectory(SaveImageUtility) add_subdirectory(RectifiedFocalLengthUtility) diff --git a/source/Utilities/LibMultiSense/MultiChannel/CMakeLists.txt b/source/Utilities/LibMultiSense/MultiChannel/CMakeLists.txt new file mode 100644 index 00000000..5e4ddf27 --- /dev/null +++ b/source/Utilities/LibMultiSense/MultiChannel/CMakeLists.txt @@ -0,0 +1,6 @@ + +add_executable(MultiChannelUtility MultiChannelUtility.cc) + +target_link_libraries (MultiChannelUtility ${MULTISENSE_UTILITY_LIBS}) + +install(TARGETS MultiChannelUtility RUNTIME DESTINATION "bin") diff --git a/source/Utilities/LibMultiSense/MultiChannel/MultiChannelUtility.cc b/source/Utilities/LibMultiSense/MultiChannel/MultiChannelUtility.cc new file mode 100644 index 00000000..1501d711 --- /dev/null +++ b/source/Utilities/LibMultiSense/MultiChannel/MultiChannelUtility.cc @@ -0,0 +1,178 @@ +/** + * @file MultiChannelUtility.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. + **/ + +#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 "getopt/getopt.h" + +namespace lms = multisense; + +namespace +{ + +volatile bool done = false; + +void usage(const char *name) +{ + std::cerr << "USAGE: " << name << " []" << std::endl; + std::cerr << "Where are:" << std::endl; + std::cerr << "\t-a
: A CURRENT IPV4 address. Supply more to synchronize outputs" << std::endl; + std::cerr << "\t-m : MTU to use to communicate with the camera (default=1500)" << std::endl; + std::cerr << "\t-t : MILLISECOND toloerance to consider frames synchronize (default=50)" << std::endl; + exit(1); +} + +#ifdef WIN32 +BOOL WINAPI signal_handler(DWORD dwCtrlType) +{ + (void) dwCtrlType; + done = true; + return TRUE; +} +#else +void signal_handler(int sig) +{ + (void) sig; + done = true; +} +#endif + +} + +int main(int argc, char** argv) +{ +#if WIN32 + SetConsoleCtrlHandler (signal_handler, TRUE); +#else + signal(SIGINT, signal_handler); +#endif + + std::vector ip_addresses; + int16_t mtu = 1500; + std::chrono::milliseconds tolerance{50}; + + int c; + while(-1 != (c = getopt(argc, argv, "a:m:r:c"))) + { + switch(c) + { + case 'a': ip_addresses.push_back(std::string(optarg)); break; + case 'm': mtu = static_cast(atoi(optarg)); break; + case 't': tolerance = std::chrono::milliseconds{atoi(optarg)}; break; + default: usage(*argv); break; + } + } + + if (ip_addresses.empty()) + { + std::cerr << "No cameras specified" << std::endl;; + } + + + std::vector> channels; + for (const auto &ip_address : ip_addresses) + { + channels.emplace_back(lms::Channel::create(lms::Channel::Config{ip_address, mtu})); + + if (!channels.back()) + { + std::cerr << "Failed to create channel" << std::endl;; + return 1; + } + + // + // QuerySet dynamic config from the camera + // + auto config = channels.back()->get_config(); + config.frames_per_second = 10.0; + if (const auto status = channels.back()->set_config(config); status != lms::Status::OK) + { + std::cerr << "Cannot set config" << std::endl; + return 1; + } + + // + // Start a single image stream + // + if (const auto status = channels.back()->start_streams({lms::DataSource::LEFT_MONO_RAW}); status != lms::Status::OK) + { + std::cerr << "Cannot start streams: " << lms::to_string(status) << std::endl; + return 1; + } + } + + auto multichannel = lms::MultiChannelSynchronizer{std::move(channels), tolerance}; + + while(!done) + { + if (const auto image_frames = multichannel.get_synchronized_frame(); image_frames) + { + for (const auto &frame : image_frames.value()) + { + std::cout << frame.frame_id << std::endl; + } + + std::cout << "done sync" << std::endl; + } + } + + for (size_t i = 0 ; i < ip_addresses.size() ; ++i) + { + multichannel.channel(i).stop_streams({lms::DataSource::ALL}); + } + + return 0; +} From f9824b00a5d8947a374843277b0dac9d66fa7dd8 Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Tue, 2 Dec 2025 18:58:52 -0500 Subject: [PATCH 02/16] Fixes for workign MultiChannel sync --- source/LibMultiSense/details/multi_channel.cc | 2 +- .../include/MultiSense/MultiSenseMultiChannel.hh | 2 +- .../LibMultiSense/MultiChannel/MultiChannelUtility.cc | 8 +++++--- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/source/LibMultiSense/details/multi_channel.cc b/source/LibMultiSense/details/multi_channel.cc index eb7b2aae..24640587 100644 --- a/source/LibMultiSense/details/multi_channel.cc +++ b/source/LibMultiSense/details/multi_channel.cc @@ -64,7 +64,7 @@ bool MultiChannelSynchronizer::frames_valid(const std::chrono::nanoseconds &tole return std::all_of(std::begin(m_active_frames), std::end(m_active_frames), [reference_time, tolerance](const auto &frame) { - return reference_time.time_since_epoch() != 0ns && (reference_time - frame.frame_time) < tolerance; + return reference_time.time_since_epoch() != 0ns && std::chrono::abs(reference_time - frame.frame_time) < tolerance; }); } diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh b/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh index 38fda41a..190e082a 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh @@ -48,9 +48,9 @@ public: explicit MultiChannelSynchronizer(std::vector> channels, const std::chrono::nanoseconds &tolerance): m_channels(std::move(channels)), - m_active_frames(channels.size()), m_tolerance(tolerance) { + m_active_frames.resize(m_channels.size()); add_user_callbacks(); } diff --git a/source/Utilities/LibMultiSense/MultiChannel/MultiChannelUtility.cc b/source/Utilities/LibMultiSense/MultiChannel/MultiChannelUtility.cc index 1501d711..3e538089 100644 --- a/source/Utilities/LibMultiSense/MultiChannel/MultiChannelUtility.cc +++ b/source/Utilities/LibMultiSense/MultiChannel/MultiChannelUtility.cc @@ -94,6 +94,8 @@ void signal_handler(int sig) int main(int argc, char** argv) { + using namespace std::chrono_literals; + #if WIN32 SetConsoleCtrlHandler (signal_handler, TRUE); #else @@ -105,7 +107,7 @@ int main(int argc, char** argv) std::chrono::milliseconds tolerance{50}; int c; - while(-1 != (c = getopt(argc, argv, "a:m:r:c"))) + while(-1 != (c = getopt(argc, argv, "a:m:t:"))) { switch(c) { @@ -158,11 +160,11 @@ int main(int argc, char** argv) while(!done) { - if (const auto image_frames = multichannel.get_synchronized_frame(); image_frames) + if (const auto image_frames = multichannel.get_synchronized_frame(500ms); image_frames) { for (const auto &frame : image_frames.value()) { - std::cout << frame.frame_id << std::endl; + std::cout << frame.frame_id << " " << frame.frame_time.time_since_epoch().count() << std::endl; } std::cout << "done sync" << std::endl; From 9a5df2d1f74ada3eabeea2d54f1b15d293879ffb Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Tue, 2 Dec 2025 21:10:31 -0500 Subject: [PATCH 03/16] Add python bindings --- python/bindings.cc | 21 +++++ .../MultiSense/MultiSenseMultiChannel.hh | 2 +- source/Utilities/LibMultiSense/CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 + .../MultiChannelUtility.cc | 0 .../multi_channel_utility.py | 87 +++++++++++++++++++ .../PointCloudUtility/point_cloud_utility.py | 4 +- 7 files changed, 114 insertions(+), 4 deletions(-) rename source/Utilities/LibMultiSense/{MultiChannel => MultiChannelUtility}/CMakeLists.txt (63%) rename source/Utilities/LibMultiSense/{MultiChannel => MultiChannelUtility}/MultiChannelUtility.cc (100%) create mode 100644 source/Utilities/LibMultiSense/MultiChannelUtility/multi_channel_utility.py diff --git a/python/bindings.cc b/python/bindings.cc index e343120c..a45ff753 100644 --- a/python/bindings.cc +++ b/python/bindings.cc @@ -44,6 +44,7 @@ #include #include +#include #include #ifdef BUILD_JSON @@ -699,6 +700,26 @@ PYBIND11_MODULE(_libmultisense, m) { .def("get_system_status", &multisense::Channel::get_system_status, py::call_guard()) .def("set_network_config", &multisense::Channel::set_network_config, py::call_guard()); + // MultiChannelSynchronizer + py::class_(m, "MultiChannelSynchronizer") + .def(py::init([](py::iterable channels, const std::chrono::nanoseconds &tolerance) { + std::vector> v; + v.reserve(py::len(channels)); + + for (py::handle obj : channels) { + v.emplace_back(obj.cast()); + } + + return new multisense::MultiChannelSynchronizer(std::move(v), tolerance); + }), + py::arg("channels"), + py::arg("tolerance") + ) + .def("channel", &multisense::MultiChannelSynchronizer::channel, py::call_guard()) + .def("get_synchronized_frame", py::overload_cast<>( &multisense::MultiChannelSynchronizer::get_synchronized_frame), py::call_guard()) + .def("get_synchronized_frame", py::overload_cast< const std::optional&>( &multisense::MultiChannelSynchronizer::get_synchronized_frame), + py::arg("timeout") = std::nullopt, py::call_guard()); + // Utilities py::class_(m, "QMatrix") .def(py::init()) diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh b/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh index 190e082a..9dbe98b9 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh @@ -69,7 +69,7 @@ public: Channel& channel(size_t index) { - if (index > m_channels.size()) + if (index >= m_channels.size()) { throw std::runtime_error("Invalid multi-channel access"); } diff --git a/source/Utilities/LibMultiSense/CMakeLists.txt b/source/Utilities/LibMultiSense/CMakeLists.txt index d677df55..405eaf0d 100644 --- a/source/Utilities/LibMultiSense/CMakeLists.txt +++ b/source/Utilities/LibMultiSense/CMakeLists.txt @@ -29,7 +29,7 @@ endif () add_subdirectory(ChangeIpUtility) add_subdirectory(DeviceInfoUtility) add_subdirectory(ImageCalUtility) -add_subdirectory(MultiChannel) +add_subdirectory(MultiChannelUtility) add_subdirectory(PointCloudUtility) add_subdirectory(SaveImageUtility) add_subdirectory(RectifiedFocalLengthUtility) diff --git a/source/Utilities/LibMultiSense/MultiChannel/CMakeLists.txt b/source/Utilities/LibMultiSense/MultiChannelUtility/CMakeLists.txt similarity index 63% rename from source/Utilities/LibMultiSense/MultiChannel/CMakeLists.txt rename to source/Utilities/LibMultiSense/MultiChannelUtility/CMakeLists.txt index 5e4ddf27..48a1f04c 100644 --- a/source/Utilities/LibMultiSense/MultiChannel/CMakeLists.txt +++ b/source/Utilities/LibMultiSense/MultiChannelUtility/CMakeLists.txt @@ -4,3 +4,5 @@ add_executable(MultiChannelUtility MultiChannelUtility.cc) target_link_libraries (MultiChannelUtility ${MULTISENSE_UTILITY_LIBS}) install(TARGETS MultiChannelUtility RUNTIME DESTINATION "bin") +install(FILES multi_channel_utility.py PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ + DESTINATION "bin") diff --git a/source/Utilities/LibMultiSense/MultiChannel/MultiChannelUtility.cc b/source/Utilities/LibMultiSense/MultiChannelUtility/MultiChannelUtility.cc similarity index 100% rename from source/Utilities/LibMultiSense/MultiChannel/MultiChannelUtility.cc rename to source/Utilities/LibMultiSense/MultiChannelUtility/MultiChannelUtility.cc diff --git a/source/Utilities/LibMultiSense/MultiChannelUtility/multi_channel_utility.py b/source/Utilities/LibMultiSense/MultiChannelUtility/multi_channel_utility.py new file mode 100644 index 00000000..bedb5f78 --- /dev/null +++ b/source/Utilities/LibMultiSense/MultiChannelUtility/multi_channel_utility.py @@ -0,0 +1,87 @@ +#!/usr/bin/env python +# +# @file multi_channel_utility.py +# +# 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. +# + +import argparse +import numpy as np + +import libmultisense as lms + +def main(args): + + channels = [] + for ip_address in args.ip_addresses: + channel_config = lms.ChannelConfig() + channel_config.ip_address = args.ip_address + channel_config.mtu = args.mtu + + channel = lms.Channel.create(channel_config) + if not channel: + print("Invalid channel") + exit(1) + + + config = channel.get_config() + config.frames_per_second = 10.0 + if channel.set_config(config) != lms.Status.OK: + print("Cannot set configuration") + exit(1) + + + if channel.start_streams([color_stream, lms.DataSource.LEFT_DISPARITY_RAW]) != lms.Status.OK: + print("Unable to start streams") + exit(1) + + channels.append(channel) + + multichannel = lms.MultiChannelSynchronizer(channels, args.tolerance) + + # milliseonds + timeout = 500 + + while True: + frames = multichannel.get_synchronized_frame(timeout) + if frames: + print("Got sync pair") + for frame in frames: + print(f"Time {frame.frame_time}") + +if __name__ == '__main__': + parser = argparse.ArgumentParser("LibMultiSense multi-channel synchronization utility") + parser.add_argument("-a", "--ip_addresses", action='append', help="The IPv4 addresses of the MultiSense to synchronize.") + parser.add_argument("-m", "--mtu", type=int, default=1500, help="The MTU to use to communicate with the camera.") + parser.add_argument("-t", "--tolerance", type=int, default=50, help="The sync tolerance in milliseconds.") + main(parser.parse_args()) diff --git a/source/Utilities/LibMultiSense/PointCloudUtility/point_cloud_utility.py b/source/Utilities/LibMultiSense/PointCloudUtility/point_cloud_utility.py index 565e7285..cff657c5 100755 --- a/source/Utilities/LibMultiSense/PointCloudUtility/point_cloud_utility.py +++ b/source/Utilities/LibMultiSense/PointCloudUtility/point_cloud_utility.py @@ -1,6 +1,6 @@ #!/usr/bin/env python # -# @file point_cloud_utility.cc +# @file point_cloud_utility.py # # Copyright 2013-2025 # Carnegie Robotics, LLC @@ -108,7 +108,7 @@ def main(args): if __name__ == '__main__': - parser = argparse.ArgumentParser("LibMultiSense save image utility") + parser = argparse.ArgumentParser("LibMultiSense save pointcloud utility") parser.add_argument("-a", "--ip_address", default="10.66.171.21", help="The IPv4 address of the MultiSense.") parser.add_argument("-m", "--mtu", type=int, default=1500, help="The MTU to use to communicate with the camera.") parser.add_argument("-r", "--max-range", type=float, default=50.0, help="The max point cloud range in meters.") From ed8cdacf814462a88b981b5dbcaf30eace73638b Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Tue, 2 Dec 2025 21:19:12 -0500 Subject: [PATCH 04/16] Fixes --- python/bindings.cc | 8 +++++++ .../multi_channel_utility.py | 21 ++++++++++--------- 2 files changed, 19 insertions(+), 10 deletions(-) diff --git a/python/bindings.cc b/python/bindings.cc index a45ff753..dc68f0e1 100644 --- a/python/bindings.cc +++ b/python/bindings.cc @@ -702,6 +702,14 @@ PYBIND11_MODULE(_libmultisense, m) { // MultiChannelSynchronizer py::class_(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) { std::vector> v; v.reserve(py::len(channels)); diff --git a/source/Utilities/LibMultiSense/MultiChannelUtility/multi_channel_utility.py b/source/Utilities/LibMultiSense/MultiChannelUtility/multi_channel_utility.py index bedb5f78..18ff3f8b 100644 --- a/source/Utilities/LibMultiSense/MultiChannelUtility/multi_channel_utility.py +++ b/source/Utilities/LibMultiSense/MultiChannelUtility/multi_channel_utility.py @@ -36,6 +36,7 @@ # import argparse +import datetime import numpy as np import libmultisense as lms @@ -45,7 +46,7 @@ def main(args): channels = [] for ip_address in args.ip_addresses: channel_config = lms.ChannelConfig() - channel_config.ip_address = args.ip_address + channel_config.ip_address = ip_address channel_config.mtu = args.mtu channel = lms.Channel.create(channel_config) @@ -67,17 +68,17 @@ def main(args): channels.append(channel) - multichannel = lms.MultiChannelSynchronizer(channels, args.tolerance) + with lms.MultiChannelSynchronizer(channels, datetime.timedelta(milliseonds=args.tolerance)) as synchronizer: - # milliseonds - timeout = 500 + # milliseonds + timeout = datetime.timedelta(milliseconds=500) - while True: - frames = multichannel.get_synchronized_frame(timeout) - if frames: - print("Got sync pair") - for frame in frames: - print(f"Time {frame.frame_time}") + while True: + frames = synchronizer.get_synchronized_frame(timeout) + if frames: + print("Got sync pair") + for frame in frames: + print(f"Time {frame.frame_time}") if __name__ == '__main__': parser = argparse.ArgumentParser("LibMultiSense multi-channel synchronization utility") From 1a6312674b7f2aad4b426783a72c5e26cad5480b Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Tue, 2 Dec 2025 23:12:32 -0500 Subject: [PATCH 05/16] Fix ownership issue with python bindings --- python/bindings.cc | 8 ++--- .../MultiSense/MultiSenseMultiChannel.hh | 36 +++++++++++++------ .../multi_channel_utility.py | 5 ++- 3 files changed, 32 insertions(+), 17 deletions(-) diff --git a/python/bindings.cc b/python/bindings.cc index dc68f0e1..c7722e27 100644 --- a/python/bindings.cc +++ b/python/bindings.cc @@ -711,14 +711,14 @@ PYBIND11_MODULE(_libmultisense, m) { return false; }) .def(py::init([](py::iterable channels, const std::chrono::nanoseconds &tolerance) { - std::vector> v; - v.reserve(py::len(channels)); + std::vector ptrs; + ptrs.reserve(py::len(channels)); for (py::handle obj : channels) { - v.emplace_back(obj.cast()); + ptrs.emplace_back(obj.cast()); } - return new multisense::MultiChannelSynchronizer(std::move(v), tolerance); + return new multisense::MultiChannelSynchronizer(std::move(ptrs), tolerance); }), py::arg("channels"), py::arg("tolerance") diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh b/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh index 9dbe98b9..f9f113a2 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh @@ -47,21 +47,23 @@ class MULTISENSE_API MultiChannelSynchronizer { public: explicit MultiChannelSynchronizer(std::vector> channels, const std::chrono::nanoseconds &tolerance): - m_channels(std::move(channels)), + m_owned_channels(std::move(channels)), m_tolerance(tolerance) { + for (auto &channel : m_owned_channels) + { + m_channels.push_back(channel.get()); + } + m_active_frames.resize(m_channels.size()); add_user_callbacks(); } - template - explicit MultiChannelSynchronizer(ChannelT&&... channels, const std::chrono::nanoseconds &tolerance): + explicit MultiChannelSynchronizer(std::vector channels, const std::chrono::nanoseconds &tolerance): + m_channels(std::move(channels)), m_tolerance(tolerance) { - m_channels.reserve(sizeof...(channels)); - m_active_frames.resize(sizeof...(channels)); - (m_channels.emplace_back(std::forward(channels)), ...); - + m_active_frames.resize(m_channels.size()); add_user_callbacks(); } @@ -91,19 +93,33 @@ private: bool frames_valid(const std::chrono::nanoseconds &tolerance); /// - /// @brief The collection of channels to synchronize + /// @brief The collection of channels raw channels to synchronize + /// + std::vector m_channels{}; + + /// + /// @brief A collection of owned channels if the user would like the synchronizer to own the channel memory /// - std::vector> m_channels{}; + std::vector> m_owned_channels{}; /// /// @brief A collection of active frames which may be dispatched to the user /// std::vector m_active_frames{}; + /// + /// @brief The max time tolerance between image frames for them to be considered equal + /// std::chrono::nanoseconds m_tolerance{}; - + /// + /// @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; }; diff --git a/source/Utilities/LibMultiSense/MultiChannelUtility/multi_channel_utility.py b/source/Utilities/LibMultiSense/MultiChannelUtility/multi_channel_utility.py index 18ff3f8b..ad781aae 100644 --- a/source/Utilities/LibMultiSense/MultiChannelUtility/multi_channel_utility.py +++ b/source/Utilities/LibMultiSense/MultiChannelUtility/multi_channel_utility.py @@ -62,15 +62,14 @@ def main(args): exit(1) - if channel.start_streams([color_stream, lms.DataSource.LEFT_DISPARITY_RAW]) != lms.Status.OK: + if channel.start_streams([lms.DataSource.LEFT_MONO_RAW]) != lms.Status.OK: print("Unable to start streams") exit(1) channels.append(channel) - with lms.MultiChannelSynchronizer(channels, datetime.timedelta(milliseonds=args.tolerance)) as synchronizer: + with lms.MultiChannelSynchronizer(channels, datetime.timedelta(milliseconds=args.tolerance)) as synchronizer: - # milliseonds timeout = datetime.timedelta(milliseconds=500) while True: From e97d0f32175c49470962c9f2474fe410d7a37d2e Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Tue, 2 Dec 2025 23:31:18 -0500 Subject: [PATCH 06/16] Refactor and docs --- python/bindings.cc | 2 + source/LibMultiSense/details/multi_channel.cc | 34 +++++++++------- .../MultiSense/MultiSenseMultiChannel.hh | 39 ++++++++++++++++++- 3 files changed, 59 insertions(+), 16 deletions(-) diff --git a/python/bindings.cc b/python/bindings.cc index c7722e27..1779ebf2 100644 --- a/python/bindings.cc +++ b/python/bindings.cc @@ -701,6 +701,8 @@ PYBIND11_MODULE(_libmultisense, m) { .def("set_network_config", &multisense::Channel::set_network_config, py::call_guard()); // MultiChannelSynchronizer + m.def("frames_synchronized", &multisense::frames_synchronized, py::call_guard()); + py::class_(m, "MultiChannelSynchronizer") .def("__enter__", [](multisense::MultiChannelSynchronizer &self) -> multisense::MultiChannelSynchronizer & { diff --git a/source/LibMultiSense/details/multi_channel.cc b/source/LibMultiSense/details/multi_channel.cc index 24640587..a2a45cc2 100644 --- a/source/LibMultiSense/details/multi_channel.cc +++ b/source/LibMultiSense/details/multi_channel.cc @@ -38,6 +38,25 @@ namespace multisense { +bool frames_synchronized(const std::vector &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.frame_time < rhs.frame_time; + }); + + return min->frame_time.time_since_epoch() != 0ns && std::chrono::abs(max->frame_time - min->frame_time) < tolerance; +} + + void MultiChannelSynchronizer::add_user_callbacks() { for(size_t i = 0 ; i < m_channels.size() ; ++i) @@ -47,7 +66,7 @@ void MultiChannelSynchronizer::add_user_callbacks() std::lock_guard lock(m_frame_mutex); m_active_frames[i] = frame; - if (frames_valid(m_tolerance)) + if (frames_synchronized(m_active_frames, m_tolerance)) { m_frame_cv.notify_all(); } @@ -55,19 +74,6 @@ void MultiChannelSynchronizer::add_user_callbacks() } } -bool MultiChannelSynchronizer::frames_valid(const std::chrono::nanoseconds &tolerance) -{ - using namespace std::chrono_literals; - - const auto reference_time = m_active_frames.front().frame_time; - - return std::all_of(std::begin(m_active_frames), std::end(m_active_frames), - [reference_time, tolerance](const auto &frame) - { - return reference_time.time_since_epoch() != 0ns && std::chrono::abs(reference_time - frame.frame_time) < tolerance; - }); -} - std::optional> MultiChannelSynchronizer::get_synchronized_frame(const std::optional &timeout) { std::unique_lock lock(m_frame_mutex); diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh b/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh index f9f113a2..a41925d0 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh @@ -43,9 +43,25 @@ 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 +/// +bool frames_synchronized(const std::vector &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 + /// explicit MultiChannelSynchronizer(std::vector> channels, const std::chrono::nanoseconds &tolerance): m_owned_channels(std::move(channels)), m_tolerance(tolerance) @@ -59,6 +75,9 @@ public: add_user_callbacks(); } + /// + /// @brief Construct a synchronizer without owning the underlying channels + /// explicit MultiChannelSynchronizer(std::vector channels, const std::chrono::nanoseconds &tolerance): m_channels(std::move(channels)), m_tolerance(tolerance) @@ -69,6 +88,9 @@ public: ~MultiChannelSynchronizer() = default; + /// + /// @brief Access a channel by index + /// Channel& channel(size_t index) { if (index >= m_channels.size()) @@ -79,19 +101,32 @@ public: 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> 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> get_synchronized_frame(const std::optional &timeout); private: + /// + /// @brief Helper to add user callbacks to the input channels + /// void add_user_callbacks(); - bool frames_valid(const std::chrono::nanoseconds &tolerance); - /// /// @brief The collection of channels raw channels to synchronize /// From 91fd7c5ebf8e6c2bc8e7fc0df4b2bd61a9fd38e9 Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Tue, 2 Dec 2025 23:39:07 -0500 Subject: [PATCH 07/16] Update prints --- CMakeLists.txt | 2 +- .../MultiChannelUtility/MultiChannelUtility.cc | 11 +++++------ .../MultiChannelUtility/multi_channel_utility.py | 6 ++---- 3 files changed, 8 insertions(+), 11 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index dba10d47..7f700766 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/source/Utilities/LibMultiSense/MultiChannelUtility/MultiChannelUtility.cc b/source/Utilities/LibMultiSense/MultiChannelUtility/MultiChannelUtility.cc index 3e538089..66f645a1 100644 --- a/source/Utilities/LibMultiSense/MultiChannelUtility/MultiChannelUtility.cc +++ b/source/Utilities/LibMultiSense/MultiChannelUtility/MultiChannelUtility.cc @@ -156,24 +156,23 @@ int main(int argc, char** argv) } } - auto multichannel = lms::MultiChannelSynchronizer{std::move(channels), tolerance}; + auto synchronizer = lms::MultiChannelSynchronizer{std::move(channels), tolerance}; while(!done) { - if (const auto image_frames = multichannel.get_synchronized_frame(500ms); image_frames) + if (const auto image_frames = synchronizer.get_synchronized_frame(500ms); image_frames) { + std::cout << "sync_group:" << std::endl; for (const auto &frame : image_frames.value()) { - std::cout << frame.frame_id << " " << frame.frame_time.time_since_epoch().count() << std::endl; + std::cout << "frame_id: " << frame.frame_id << " time: " << frame.frame_time.time_since_epoch().count() << std::endl; } - - std::cout << "done sync" << std::endl; } } for (size_t i = 0 ; i < ip_addresses.size() ; ++i) { - multichannel.channel(i).stop_streams({lms::DataSource::ALL}); + synchronizer.channel(i).stop_streams({lms::DataSource::ALL}); } return 0; diff --git a/source/Utilities/LibMultiSense/MultiChannelUtility/multi_channel_utility.py b/source/Utilities/LibMultiSense/MultiChannelUtility/multi_channel_utility.py index ad781aae..2a4d1f01 100644 --- a/source/Utilities/LibMultiSense/MultiChannelUtility/multi_channel_utility.py +++ b/source/Utilities/LibMultiSense/MultiChannelUtility/multi_channel_utility.py @@ -69,15 +69,13 @@ def main(args): channels.append(channel) with lms.MultiChannelSynchronizer(channels, datetime.timedelta(milliseconds=args.tolerance)) as synchronizer: - timeout = datetime.timedelta(milliseconds=500) - while True: frames = synchronizer.get_synchronized_frame(timeout) if frames: - print("Got sync pair") + print("sync group:") for frame in frames: - print(f"Time {frame.frame_time}") + print(f"frame_id: {frame.frame_id} time: {frame.frame_time}") if __name__ == '__main__': parser = argparse.ArgumentParser("LibMultiSense multi-channel synchronization utility") From 1efb0148db00672f3febb974d84d8fd4b3b11e87 Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Tue, 2 Dec 2025 23:50:40 -0500 Subject: [PATCH 08/16] Add sync unit test --- source/LibMultiSense/test/CMakeLists.txt | 1 + .../LibMultiSense/test/multi_channel_test.cc | 61 +++++++++++++++++++ 2 files changed, 62 insertions(+) create mode 100644 source/LibMultiSense/test/multi_channel_test.cc diff --git a/source/LibMultiSense/test/CMakeLists.txt b/source/LibMultiSense/test/CMakeLists.txt index 52c81411..a7e7bcf3 100644 --- a/source/LibMultiSense/test/CMakeLists.txt +++ b/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 diff --git a/source/LibMultiSense/test/multi_channel_test.cc b/source/LibMultiSense/test/multi_channel_test.cc new file mode 100644 index 00000000..49500715 --- /dev/null +++ b/source/LibMultiSense/test/multi_channel_test.cc @@ -0,0 +1,61 @@ +/** + * @file multi_channel_test.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 + +#include + +using namespace multisense; + +TEST(frames_synchronized, basic_test) +{ + using namespace std::chrono_literals; + std::vector frames{}; + + EXPECT_TRUE(frames_synchronized(frames, 0ns)); + + frames.emplace_back(ImageFrame{1, {}, {}, TimeT{10ms}, TimeT{10ms}, {}, {}, {}, {}}); + frames.emplace_back(ImageFrame{3, {}, {}, TimeT{30ms}, TimeT{30ms}, {}, {}, {}, {}}); + + EXPECT_FALSE(frames_synchronized(frames, 0ns)); + EXPECT_TRUE(frames_synchronized(frames, 21ms)); + + frames.emplace_back(ImageFrame{37, {}, {}, TimeT{38ms}, TimeT{38ms}, {}, {}, {}, {}}); + frames.emplace_back(ImageFrame{7, {}, {}, TimeT{31ms}, TimeT{31ms}, {}, {}, {}, {}}); + + EXPECT_FALSE(frames_synchronized(frames, 21ms)); + EXPECT_TRUE(frames_synchronized(frames, 29ms)); +} From d9f349256f7efbc3aa8c0d5ce268749364b21aa2 Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Wed, 3 Dec 2025 01:02:34 -0500 Subject: [PATCH 09/16] Add missing header --- source/LibMultiSense/details/multi_channel.cc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/source/LibMultiSense/details/multi_channel.cc b/source/LibMultiSense/details/multi_channel.cc index a2a45cc2..0c14f7f8 100644 --- a/source/LibMultiSense/details/multi_channel.cc +++ b/source/LibMultiSense/details/multi_channel.cc @@ -34,6 +34,8 @@ * 2025-12-02, malvarado@carnegierobotics.com, IRAD, Created file. **/ +#include + #include "MultiSense/MultiSenseMultiChannel.hh" namespace multisense { From dcd58ae6fde85b2ba550f07562e7c17b03025f60 Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Wed, 3 Dec 2025 10:01:28 -0500 Subject: [PATCH 10/16] Fix windows build --- .../LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh b/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh index a41925d0..652da347 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh @@ -51,7 +51,7 @@ namespace multisense { /// /// @return Return true if the frames are synchronized /// -bool frames_synchronized(const std::vector &frames, const std::chrono::nanoseconds &tolerance); +MULTISENSE_API bool frames_synchronized(const std::vector &frames, const std::chrono::nanoseconds &tolerance); /// /// @brief Helper class which provides a interface to synchronize data across multiple channels From 87faebba96a4834974e07cc894f1c8d455690d0c Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Fri, 5 Dec 2025 23:23:58 -0500 Subject: [PATCH 11/16] Add a deque of sync frames so we don't miss any --- source/LibMultiSense/details/multi_channel.cc | 17 ++++++++++++----- .../MultiSense/MultiSenseMultiChannel.hh | 8 +++++++- 2 files changed, 19 insertions(+), 6 deletions(-) diff --git a/source/LibMultiSense/details/multi_channel.cc b/source/LibMultiSense/details/multi_channel.cc index 0c14f7f8..3cd6aef3 100644 --- a/source/LibMultiSense/details/multi_channel.cc +++ b/source/LibMultiSense/details/multi_channel.cc @@ -70,6 +70,12 @@ void MultiChannelSynchronizer::add_user_callbacks() if (frames_synchronized(m_active_frames, m_tolerance)) { + m_ready_frames.emplace_back(m_active_frames); + for (auto &active_frame : m_active_frames) + { + active_frame = ImageFrame{}; + } + m_frame_cv.notify_all(); } }); @@ -79,21 +85,22 @@ void MultiChannelSynchronizer::add_user_callbacks() std::optional> MultiChannelSynchronizer::get_synchronized_frame(const std::optional &timeout) { std::unique_lock lock(m_frame_mutex); + const auto frames_ready = [this]() { return !m_ready_frames.empty(); }; + if (timeout) { - if (std::cv_status::timeout == m_frame_cv.wait_for(lock, timeout.value())) + if (!m_frame_cv.wait_for(lock, timeout.value(), frames_ready)) { return std::nullopt; } } else { - m_frame_cv.wait(lock); + m_frame_cv.wait(lock, frames_ready); } - auto output_frames = m_active_frames; - m_active_frames.clear(); - m_active_frames.resize(m_channels.size()); + auto output_frames = std::move(m_ready_frames.front()); + m_ready_frames.pop_front(); return output_frames; } diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh b/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh index 652da347..fd83e568 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh @@ -36,8 +36,9 @@ #pragma once -#include #include +#include +#include #include "MultiSenseChannel.hh" @@ -156,6 +157,11 @@ private: /// @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> m_ready_frames{}; }; } From 5491da54bf28aac39704e5ed9b77df4b94714d1c Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Fri, 5 Dec 2025 23:41:35 -0500 Subject: [PATCH 12/16] Limit the number of queued frames --- source/LibMultiSense/details/multi_channel.cc | 6 ++++ .../MultiSense/MultiSenseMultiChannel.hh | 29 +++++++++++++++---- 2 files changed, 30 insertions(+), 5 deletions(-) diff --git a/source/LibMultiSense/details/multi_channel.cc b/source/LibMultiSense/details/multi_channel.cc index 3cd6aef3..7bc731ac 100644 --- a/source/LibMultiSense/details/multi_channel.cc +++ b/source/LibMultiSense/details/multi_channel.cc @@ -70,6 +70,12 @@ void MultiChannelSynchronizer::add_user_callbacks() 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) { diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh b/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh index fd83e568..32a4778e 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh @@ -61,11 +61,18 @@ class MULTISENSE_API MultiChannelSynchronizer { public: /// - /// @brief Construct a synchronizer owning the underlying channels + /// @brief Construct a synchronizer owning the underlying channels /// - explicit MultiChannelSynchronizer(std::vector> channels, const std::chrono::nanoseconds &tolerance): + /// @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> channels, + const std::chrono::nanoseconds &tolerance, + size_t max_queue_size = 0): m_owned_channels(std::move(channels)), - m_tolerance(tolerance) + m_tolerance(tolerance), + m_max_queue_size(max_queue_size) { for (auto &channel : m_owned_channels) { @@ -79,9 +86,16 @@ public: /// /// @brief Construct a synchronizer without owning the underlying channels /// - explicit MultiChannelSynchronizer(std::vector channels, const std::chrono::nanoseconds &tolerance): + /// @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 channels, + const std::chrono::nanoseconds &tolerance, + size_t max_queue_size = 0): m_channels(std::move(channels)), - m_tolerance(tolerance) + m_tolerance(tolerance), + m_max_queue_size(max_queue_size) { m_active_frames.resize(m_channels.size()); add_user_callbacks(); @@ -148,6 +162,11 @@ private: /// 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 /// From 958ad7b7fd875f8e583db3ce83494f6f3f619eec Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Fri, 5 Dec 2025 23:47:27 -0500 Subject: [PATCH 13/16] Revert bindings change --- python/bindings.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/python/bindings.cc b/python/bindings.cc index 1779ebf2..45af4047 100644 --- a/python/bindings.cc +++ b/python/bindings.cc @@ -723,7 +723,8 @@ PYBIND11_MODULE(_libmultisense, m) { return new multisense::MultiChannelSynchronizer(std::move(ptrs), tolerance); }), py::arg("channels"), - py::arg("tolerance") + py::arg("tolerance"), + py::arg("max_queue_size") ) .def("channel", &multisense::MultiChannelSynchronizer::channel, py::call_guard()) .def("get_synchronized_frame", py::overload_cast<>( &multisense::MultiChannelSynchronizer::get_synchronized_frame), py::call_guard()) From c0456e944f4f97aedad2e003892138cd4e5465ea Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Fri, 5 Dec 2025 23:54:05 -0500 Subject: [PATCH 14/16] Fix bindings --- python/bindings.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/python/bindings.cc b/python/bindings.cc index 45af4047..f5552962 100644 --- a/python/bindings.cc +++ b/python/bindings.cc @@ -712,7 +712,7 @@ PYBIND11_MODULE(_libmultisense, m) { { return false; }) - .def(py::init([](py::iterable channels, const std::chrono::nanoseconds &tolerance) { + .def(py::init([](py::iterable channels, const std::chrono::nanoseconds &tolerance, size_t max_queue_size) { std::vector ptrs; ptrs.reserve(py::len(channels)); @@ -720,11 +720,11 @@ PYBIND11_MODULE(_libmultisense, m) { ptrs.emplace_back(obj.cast()); } - return new multisense::MultiChannelSynchronizer(std::move(ptrs), tolerance); + return new multisense::MultiChannelSynchronizer(std::move(ptrs), tolerance, max_queue_size); }), py::arg("channels"), py::arg("tolerance"), - py::arg("max_queue_size") + py::arg("max_queue_size") = 0 ) .def("channel", &multisense::MultiChannelSynchronizer::channel, py::call_guard()) .def("get_synchronized_frame", py::overload_cast<>( &multisense::MultiChannelSynchronizer::get_synchronized_frame), py::call_guard()) From c8679a51224a4e5cde47cd0a1492047267686671 Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Mon, 15 Dec 2025 15:29:49 -0500 Subject: [PATCH 15/16] Add checks to force us to use PTP --- source/LibMultiSense/details/multi_channel.cc | 17 ++++++++++++++--- .../MultiSense/MultiSenseMultiChannel.hh | 9 ++++++++- 2 files changed, 22 insertions(+), 4 deletions(-) diff --git a/source/LibMultiSense/details/multi_channel.cc b/source/LibMultiSense/details/multi_channel.cc index 7bc731ac..16c9656c 100644 --- a/source/LibMultiSense/details/multi_channel.cc +++ b/source/LibMultiSense/details/multi_channel.cc @@ -52,10 +52,11 @@ bool frames_synchronized(const std::vector &frames, const std::chron const auto [min, max] = std::minmax_element(std::begin(frames), std::end(frames), [](const auto &lhs, const auto &rhs) { - return lhs.frame_time < rhs.frame_time; + return lhs.ptp_frame_time < rhs.ptp_frame_time; }); - return min->frame_time.time_since_epoch() != 0ns && std::chrono::abs(max->frame_time - min->frame_time) < tolerance; + return min->ptp_frame_time.time_since_epoch() != 0ns && std::chrono::abs(max->ptp_frame_time - min->ptp_frame_time) < tolerance; + } @@ -66,7 +67,12 @@ void MultiChannelSynchronizer::add_user_callbacks() m_channels[i]->add_image_frame_callback([i, this](auto frame) { std::lock_guard lock(m_frame_mutex); - m_active_frames[i] = frame; + 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)) { @@ -105,6 +111,11 @@ std::optional> MultiChannelSynchronizer::get_synchronize 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; diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh b/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh index 32a4778e..7674eaa0 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh @@ -55,7 +55,7 @@ namespace multisense { MULTISENSE_API bool frames_synchronized(const std::vector &frames, const std::chrono::nanoseconds &tolerance); /// -/// @brief Helper class which provides a interface to synchronize data across multiple channels +/// @brief Helper class which provides a interface to synchronize data across multiple channels. /// class MULTISENSE_API MultiChannelSynchronizer { public: @@ -76,6 +76,13 @@ public: { 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()); } From 076ae4d64e95a0b86e48197939d54b810260c7af Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Tue, 16 Dec 2025 22:42:59 -0500 Subject: [PATCH 16/16] Updates to enable ptp in examples --- python/bindings.cc | 2 +- .../LibMultiSense/MultiChannelUtility/MultiChannelUtility.cc | 5 +++++ .../MultiChannelUtility/multi_channel_utility.py | 1 + 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/python/bindings.cc b/python/bindings.cc index 87297b3a..8b910d69 100644 --- a/python/bindings.cc +++ b/python/bindings.cc @@ -725,7 +725,7 @@ PYBIND11_MODULE(_libmultisense, m) { { return false; }) - .def(py::init([](py::iterable channels, const std::chrono::nanoseconds &tolerance, size_t max_queue_size) { + .def(py::init([](py::iterable channels, const std::chrono::nanoseconds &tolerance, py::ssize_t max_queue_size) { std::vector ptrs; ptrs.reserve(py::len(channels)); diff --git a/source/Utilities/LibMultiSense/MultiChannelUtility/MultiChannelUtility.cc b/source/Utilities/LibMultiSense/MultiChannelUtility/MultiChannelUtility.cc index 66f645a1..81f0756c 100644 --- a/source/Utilities/LibMultiSense/MultiChannelUtility/MultiChannelUtility.cc +++ b/source/Utilities/LibMultiSense/MultiChannelUtility/MultiChannelUtility.cc @@ -140,6 +140,11 @@ int main(int argc, char** argv) // auto config = channels.back()->get_config(); config.frames_per_second = 10.0; + if (config.time_config) + { + config.time_config->ptp_enabled = true; + } + if (const auto status = channels.back()->set_config(config); status != lms::Status::OK) { std::cerr << "Cannot set config" << std::endl; diff --git a/source/Utilities/LibMultiSense/MultiChannelUtility/multi_channel_utility.py b/source/Utilities/LibMultiSense/MultiChannelUtility/multi_channel_utility.py index 2a4d1f01..4088fe1e 100644 --- a/source/Utilities/LibMultiSense/MultiChannelUtility/multi_channel_utility.py +++ b/source/Utilities/LibMultiSense/MultiChannelUtility/multi_channel_utility.py @@ -57,6 +57,7 @@ def main(args): config = channel.get_config() config.frames_per_second = 10.0 + config.time_config.ptp_enabled = True if channel.set_config(config) != lms.Status.OK: print("Cannot set configuration") exit(1)