diff --git a/CMakeLists.txt b/CMakeLists.txt index 073f2558..8191d453 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/python/bindings.cc b/python/bindings.cc index cde6995c..8b910d69 100644 --- a/python/bindings.cc +++ b/python/bindings.cc @@ -44,6 +44,7 @@ #include #include +#include #include #ifdef BUILD_JSON @@ -712,6 +713,37 @@ 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 + m.def("frames_synchronized", &multisense::frames_synchronized, py::call_guard()); + + 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, py::ssize_t max_queue_size) { + std::vector ptrs; + ptrs.reserve(py::len(channels)); + + for (py::handle obj : channels) { + ptrs.emplace_back(obj.cast()); + } + + 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()) + .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/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..16c9656c --- /dev/null +++ b/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 + +#include "MultiSense/MultiSenseMultiChannel.hh" + +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.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 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> 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 (!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; +} + +} diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh b/source/LibMultiSense/include/MultiSense/MultiSenseMultiChannel.hh new file mode 100644 index 00000000..7674eaa0 --- /dev/null +++ b/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 +#include +#include + +#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 &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> 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 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> 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(); + + /// + /// @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_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 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> m_ready_frames{}; +}; + +} 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)); +} diff --git a/source/Utilities/LibMultiSense/CMakeLists.txt b/source/Utilities/LibMultiSense/CMakeLists.txt index 3d21f9be..405eaf0d 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(MultiChannelUtility) add_subdirectory(PointCloudUtility) add_subdirectory(SaveImageUtility) add_subdirectory(RectifiedFocalLengthUtility) diff --git a/source/Utilities/LibMultiSense/MultiChannelUtility/CMakeLists.txt b/source/Utilities/LibMultiSense/MultiChannelUtility/CMakeLists.txt new file mode 100644 index 00000000..48a1f04c --- /dev/null +++ b/source/Utilities/LibMultiSense/MultiChannelUtility/CMakeLists.txt @@ -0,0 +1,8 @@ + +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/MultiChannelUtility/MultiChannelUtility.cc b/source/Utilities/LibMultiSense/MultiChannelUtility/MultiChannelUtility.cc new file mode 100644 index 00000000..81f0756c --- /dev/null +++ b/source/Utilities/LibMultiSense/MultiChannelUtility/MultiChannelUtility.cc @@ -0,0 +1,184 @@ +/** + * @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) +{ + using namespace std::chrono_literals; + +#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:t:"))) + { + 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 (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; + 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 synchronizer = lms::MultiChannelSynchronizer{std::move(channels), tolerance}; + + while(!done) + { + 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_id: " << frame.frame_id << " time: " << frame.frame_time.time_since_epoch().count() << std::endl; + } + } + } + + for (size_t i = 0 ; i < ip_addresses.size() ; ++i) + { + 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 new file mode 100644 index 00000000..4088fe1e --- /dev/null +++ b/source/Utilities/LibMultiSense/MultiChannelUtility/multi_channel_utility.py @@ -0,0 +1,86 @@ +#!/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 datetime +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 = 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 + config.time_config.ptp_enabled = True + if channel.set_config(config) != lms.Status.OK: + print("Cannot set configuration") + exit(1) + + + 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(milliseconds=args.tolerance)) as synchronizer: + timeout = datetime.timedelta(milliseconds=500) + while True: + frames = synchronizer.get_synchronized_frame(timeout) + if frames: + print("sync group:") + for frame in frames: + print(f"frame_id: {frame.frame_id} 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.")