Add ptp utility

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)

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 ptp_utility.py PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ
+        DESTINATION "bin")

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 <windows.h>
+#include <winsock2.h>
+#else
+#include <unistd.h>
+#endif
+
+#include <csignal>
+#include <iostream>
+#include <thread>
+
+#include <MultiSense/MultiSenseChannel.hh>
+#include <MultiSense/MultiSenseUtilities.hh>
+
+#include "getopt/getopt.h"
+
+namespace lms = multisense;
+
+namespace
+{
+
+volatile bool done = false;
+
+void usage(const char *name)
+{
+    std::cerr << "USAGE: " << name << " [<options>]" << std::endl;
+    std::cerr << "Where <options> are:" << std::endl;
+    std::cerr << "\t-a <current_address> : CURRENT IPV4 address (default=10.66.171.21)" << std::endl;
+    std::cerr << "\t-m <mtu>             : 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<uint16_t>(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 << "Acquired image header to current time offset (ns): " <<
+                std::chrono::duration_cast<std::chrono::nanoseconds>(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<int>(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;
+}

source/Utilities/LibMultiSense/PtpUtility/ptp_utility.py

@@ -0,0 +1,99 @@
+#!/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(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 (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
+
+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).total_seconds()
+            print(f"Acquired image header to current time offset (s): {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())

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;