From 25bb3516d31f5f9fcee2c50a6ddd7e5956fdc357 Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Thu, 18 Dec 2025 08:56:24 -0500 Subject: [PATCH 1/5] Add PTP utility to check PTP status of the camera --- source/Utilities/LibMultiSense/CMakeLists.txt | 1 + .../LibMultiSense/PtpUtility/CMakeLists.txt | 8 + .../LibMultiSense/PtpUtility/PtpUtility.cc | 184 ++++++++++++++++++ .../LibMultiSense/PtpUtility/ptp_utility.py | 98 ++++++++++ .../SaveImageUtility/SaveImageUtility.cc | 3 - 5 files changed, 291 insertions(+), 3 deletions(-) create mode 100644 source/Utilities/LibMultiSense/PtpUtility/CMakeLists.txt create mode 100644 source/Utilities/LibMultiSense/PtpUtility/PtpUtility.cc create mode 100755 source/Utilities/LibMultiSense/PtpUtility/ptp_utility.py diff --git a/source/Utilities/LibMultiSense/CMakeLists.txt b/source/Utilities/LibMultiSense/CMakeLists.txt index 405eaf0d..a5344e7b 100644 --- a/source/Utilities/LibMultiSense/CMakeLists.txt +++ b/source/Utilities/LibMultiSense/CMakeLists.txt @@ -31,6 +31,7 @@ add_subdirectory(DeviceInfoUtility) add_subdirectory(ImageCalUtility) add_subdirectory(MultiChannelUtility) add_subdirectory(PointCloudUtility) +add_subdirectory(PtpUtility) add_subdirectory(SaveImageUtility) add_subdirectory(RectifiedFocalLengthUtility) add_subdirectory(VersionInfoUtility) diff --git a/source/Utilities/LibMultiSense/PtpUtility/CMakeLists.txt b/source/Utilities/LibMultiSense/PtpUtility/CMakeLists.txt new file mode 100644 index 00000000..754a5193 --- /dev/null +++ b/source/Utilities/LibMultiSense/PtpUtility/CMakeLists.txt @@ -0,0 +1,8 @@ + +add_executable(PtpUtility PtpUtility.cc) + +target_link_libraries (PtpUtility ${MULTISENSE_UTILITY_LIBS}) + +install(TARGETS PtpUtility RUNTIME DESTINATION "bin") +install(FILES version_info_utility.py PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ + DESTINATION "bin") diff --git a/source/Utilities/LibMultiSense/PtpUtility/PtpUtility.cc b/source/Utilities/LibMultiSense/PtpUtility/PtpUtility.cc new file mode 100644 index 00000000..a48a722f --- /dev/null +++ b/source/Utilities/LibMultiSense/PtpUtility/PtpUtility.cc @@ -0,0 +1,184 @@ +/** + * @file PtpUtility.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-18, 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 "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 : CURRENT IPV4 address (default=10.66.171.21)" << std::endl; + std::cerr << "\t-m : MTU to use to communicate with the camera (default=1500)" << 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::string ip_address = "10.66.171.21"; + int16_t mtu = 1500; + + int c; + while(-1 != (c = getopt(argc, argv, "a:m:"))) + { + switch(c) + { + case 'a': ip_address = std::string(optarg); break; + case 'm': mtu = static_cast(atoi(optarg)); break; + default: usage(*argv); break; + } + } + + const auto channel = lms::Channel::create(lms::Channel::Config{ip_address, mtu}); + if (!channel) + { + std::cerr << "Failed to create channel" << std::endl;; + return 1; + } + + auto config = channel->get_config(); + if (!config.time_config) + { + std::cerr << "Camera does not support PTP" << std::endl; + return 1; + } + config.time_config->ptp_enabled = true; + if (const auto status = channel->set_config(config); status != lms::Status::OK) + { + std::cerr << "Cannot set config: " << lms::to_string(status) << std::endl; + return 1; + } + + // + // Start a single image stream to check timestamps against + // + if (const auto status = channel->start_streams({lms::DataSource::LEFT_RECTIFIED_RAW}); status != lms::Status::OK) + { + std::cerr << "Cannot start streams: " << lms::to_string(status) << std::endl; + return 1; + } + + while(!done) + { + if (const auto image_frame = channel->get_next_image_frame(); image_frame) + { + const auto now = std::chrono::system_clock::now(); + + std::cout << "Approximate time offset between camera and system (ns): " << + std::chrono::duration_cast(image_frame->ptp_frame_time - now).count() << std::endl; + } + + if (const auto status = channel->get_system_status(); status) + { + if (status->ptp) + { + std::cout << "Grandmaster present: " << status->ptp->grandmaster_present << ", "; + std::cout << "Grandmaster id: ["; + for (const auto &id : status->ptp->grandmaster_id) + { + std::cout << static_cast(id) << ", "; + } + std::cout << "], "; + std::cout << "Grandmaster offset (ns): " << status->ptp->grandmaster_offset.count() << ", "; + std::cout << "Path delay (ns): " << status->ptp->path_delay.count() << ", "; + std::cout << "Steps from local to grandmaster: " << status->ptp->steps_from_local_to_grandmaster << std::endl; + } + else + { + std::cout << "PTP not supported" << std::endl; + } + } + else + { + std::cerr << "Failed to query sensor status" << std::endl; + } + + std::this_thread::sleep_for(1s); + } + + channel->stop_streams({lms::DataSource::ALL}); + + return 0; +} diff --git a/source/Utilities/LibMultiSense/PtpUtility/ptp_utility.py b/source/Utilities/LibMultiSense/PtpUtility/ptp_utility.py new file mode 100755 index 00000000..b9bf822e --- /dev/null +++ b/source/Utilities/LibMultiSense/PtpUtility/ptp_utility.py @@ -0,0 +1,98 @@ +#!/usr/bin/env python +# +# @file version_info_utility.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-02-07, malvarado@carnegierobotics.com, IRAD, Created file. +# + +import argparse +import datetime +import time + +import libmultisense as lms + +def get_ptp_status_string(status): + if status.ptp is None: + return "PTP not supported" + + grandmaster_id = ",".join(status.ptp.grandmaster_id) + output = f"Grandmaster present: {status.ptp.grandmaster_present}, "\ + f"Grandmaster id: {grandmaster_id}," \ + f"Grandmaster offset: {status.ptp.grandmaster_offset}," \ + f"Path delay: {status.ptp.path_delay}," \ + f"Steps from local to grandmaster: {status.ptp.steps_from_local_to_grandmaster}" + + return output + +def main(args): + channel_config = lms.ChannelConfig() + channel_config.ip_address = args.ip_address + channel_config.mtu = args.mtu + + with lms.Channel.create(channel_config) as channel: + if not channel: + print("Invalid channel") + exit(1) + + config = channel.get_config() + if config.time_config is None: + print("Camera does not support PTP") + exit(1) + config.time_config.ptp_enabled = True + status = channel.set_config(config) + if status != lms.Status.OK: + print("Cannot set configuration", lms.to_string(status)) + exit(1) + + if channel.start_streams([lms.DataSource.LEFT_RECTIFIED_RAW]) != lms.Status.OK: + print("Unable to start streams") + exit(1) + + while True: + frame = channel.get_next_image_frame() + if frame: + now = datetime.datetime.now() + delay = frame.ptp_frame_time - now + print(f"Approximate time offset between camera and system: {delay}") + + status = channel.get_system_status() + if status: + print(get_ptp_status_string(status)) + + time.sleep(1) + +if __name__ == '__main__': + parser = argparse.ArgumentParser("LibMultiSense version info 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.") + main(parser.parse_args()) diff --git a/source/Utilities/LibMultiSense/SaveImageUtility/SaveImageUtility.cc b/source/Utilities/LibMultiSense/SaveImageUtility/SaveImageUtility.cc index 7a03f902..31e42f08 100644 --- a/source/Utilities/LibMultiSense/SaveImageUtility/SaveImageUtility.cc +++ b/source/Utilities/LibMultiSense/SaveImageUtility/SaveImageUtility.cc @@ -217,9 +217,6 @@ int main(int argc, char** argv) return 0; } - // - // Start a single image stream - // if (const auto status = channel->start_streams(image_streams); status != lms::Status::OK) { std::cerr << "Cannot start streams: " << lms::to_string(status) << std::endl; From a2c9f9eb557d09a7d8d53230634c6acb92ea9316 Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Thu, 18 Dec 2025 09:01:51 -0500 Subject: [PATCH 2/5] Fix join operation --- source/Utilities/LibMultiSense/PtpUtility/ptp_utility.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/source/Utilities/LibMultiSense/PtpUtility/ptp_utility.py b/source/Utilities/LibMultiSense/PtpUtility/ptp_utility.py index b9bf822e..f5821c53 100755 --- a/source/Utilities/LibMultiSense/PtpUtility/ptp_utility.py +++ b/source/Utilities/LibMultiSense/PtpUtility/ptp_utility.py @@ -45,7 +45,8 @@ def get_ptp_status_string(status): if status.ptp is None: return "PTP not supported" - grandmaster_id = ",".join(status.ptp.grandmaster_id) + grandmaster_id = ",".join(str(i) for i in status.ptp.grandmaster_id) + output = f"Grandmaster present: {status.ptp.grandmaster_present}, "\ f"Grandmaster id: {grandmaster_id}," \ f"Grandmaster offset: {status.ptp.grandmaster_offset}," \ From f0d9ac3b9c75f89619d2d4bd03b3678753840180 Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Thu, 18 Dec 2025 09:05:55 -0500 Subject: [PATCH 3/5] Print out times in seconds --- .../Utilities/LibMultiSense/PtpUtility/ptp_utility.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/source/Utilities/LibMultiSense/PtpUtility/ptp_utility.py b/source/Utilities/LibMultiSense/PtpUtility/ptp_utility.py index f5821c53..b225b544 100755 --- a/source/Utilities/LibMultiSense/PtpUtility/ptp_utility.py +++ b/source/Utilities/LibMultiSense/PtpUtility/ptp_utility.py @@ -48,9 +48,9 @@ def get_ptp_status_string(status): grandmaster_id = ",".join(str(i) for i in status.ptp.grandmaster_id) output = f"Grandmaster present: {status.ptp.grandmaster_present}, "\ - f"Grandmaster id: {grandmaster_id}," \ - f"Grandmaster offset: {status.ptp.grandmaster_offset}," \ - f"Path delay: {status.ptp.path_delay}," \ + f"Grandmaster id: {grandmaster_id}, " \ + f"Grandmaster offset (s): {status.ptp.grandmaster_offset.total_seconds()}, " \ + f"Path delay (s): {status.ptp.path_delay.total_seconds()}, " \ f"Steps from local to grandmaster: {status.ptp.steps_from_local_to_grandmaster}" return output @@ -83,8 +83,8 @@ def main(args): frame = channel.get_next_image_frame() if frame: now = datetime.datetime.now() - delay = frame.ptp_frame_time - now - print(f"Approximate time offset between camera and system: {delay}") + delay = (frame.ptp_frame_time - now).total_seconds() + print(f"Approximate time offset between camera and system (s): {delay}") status = channel.get_system_status() if status: From badf9d740360c2404e57a5f5f63d88098575906b Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Thu, 18 Dec 2025 09:16:38 -0500 Subject: [PATCH 4/5] Update install path for python --- source/Utilities/LibMultiSense/PtpUtility/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/Utilities/LibMultiSense/PtpUtility/CMakeLists.txt b/source/Utilities/LibMultiSense/PtpUtility/CMakeLists.txt index 754a5193..12526239 100644 --- a/source/Utilities/LibMultiSense/PtpUtility/CMakeLists.txt +++ b/source/Utilities/LibMultiSense/PtpUtility/CMakeLists.txt @@ -4,5 +4,5 @@ add_executable(PtpUtility PtpUtility.cc) target_link_libraries (PtpUtility ${MULTISENSE_UTILITY_LIBS}) install(TARGETS PtpUtility RUNTIME DESTINATION "bin") -install(FILES version_info_utility.py PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ +install(FILES ptp_utility.py PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ DESTINATION "bin") From 404144406eb69d8175c6f256024fa81a89cac829 Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Thu, 22 Jan 2026 22:18:26 -0500 Subject: [PATCH 5/5] Address review comments --- source/Utilities/LibMultiSense/PtpUtility/PtpUtility.cc | 2 +- source/Utilities/LibMultiSense/PtpUtility/ptp_utility.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/source/Utilities/LibMultiSense/PtpUtility/PtpUtility.cc b/source/Utilities/LibMultiSense/PtpUtility/PtpUtility.cc index a48a722f..821ddabf 100644 --- a/source/Utilities/LibMultiSense/PtpUtility/PtpUtility.cc +++ b/source/Utilities/LibMultiSense/PtpUtility/PtpUtility.cc @@ -146,7 +146,7 @@ int main(int argc, char** argv) { const auto now = std::chrono::system_clock::now(); - std::cout << "Approximate time offset between camera and system (ns): " << + std::cout << "Acquired image header to current time offset (ns): " << std::chrono::duration_cast(image_frame->ptp_frame_time - now).count() << std::endl; } diff --git a/source/Utilities/LibMultiSense/PtpUtility/ptp_utility.py b/source/Utilities/LibMultiSense/PtpUtility/ptp_utility.py index b225b544..51810298 100755 --- a/source/Utilities/LibMultiSense/PtpUtility/ptp_utility.py +++ b/source/Utilities/LibMultiSense/PtpUtility/ptp_utility.py @@ -84,7 +84,7 @@ def main(args): if frame: now = datetime.datetime.now() delay = (frame.ptp_frame_time - now).total_seconds() - print(f"Approximate time offset between camera and system (s): {delay}") + print(f"Acquired image header to current time offset (s): {delay}") status = channel.get_system_status() if status: