From fa7b4173643bb643b19d42e06ea557c9fe9d0dcf Mon Sep 17 00:00:00 2001 From: Xinghua Sun Date: Tue, 1 Sep 2026 13:23:14 -0700 Subject: [PATCH 1/2] Estimate device-to-host clock offset and skew over a sliding window, detect stale calibrations and device restarts, and publish immutable snapshots for non-blocking timestamp conversion. include C++ and python bindings with synchronization and tests Signed-off-by: Xinghua Sun --- src/core/CMakeLists.txt | 3 + src/core/python/CMakeLists.txt | 2 +- src/core/synchronization/CMakeLists.txt | 11 + src/core/synchronization/cpp/CMakeLists.txt | 22 + .../cpp/device_clock_estimator.cpp | 284 ++++++++++++ .../device_clock_estimator.hpp | 210 +++++++++ .../synchronization/python/CMakeLists.txt | 17 + .../python/synchronization_bindings.cpp | 85 ++++ .../isaacteleop/synchronization/__init__.py | 31 ++ tests/cpp/core/CMakeLists.txt | 1 + tests/cpp/core/synchronization/CMakeLists.txt | 19 + .../test_device_clock_estimator.cpp | 413 ++++++++++++++++++ tests/python/core/CMakeLists.txt | 1 + .../core/synchronization/CMakeLists.txt | 29 ++ .../core/synchronization/pyproject.toml | 23 + .../test_device_clock_estimator.py | 149 +++++++ 16 files changed, 1299 insertions(+), 1 deletion(-) create mode 100644 src/core/synchronization/CMakeLists.txt create mode 100644 src/core/synchronization/cpp/CMakeLists.txt create mode 100644 src/core/synchronization/cpp/device_clock_estimator.cpp create mode 100644 src/core/synchronization/cpp/inc/synchronization/device_clock_estimator.hpp create mode 100644 src/core/synchronization/python/CMakeLists.txt create mode 100644 src/core/synchronization/python/synchronization_bindings.cpp create mode 100644 src/python/isaacteleop/synchronization/__init__.py create mode 100644 tests/cpp/core/synchronization/CMakeLists.txt create mode 100644 tests/cpp/core/synchronization/test_device_clock_estimator.cpp create mode 100644 tests/python/core/synchronization/CMakeLists.txt create mode 100644 tests/python/core/synchronization/pyproject.toml create mode 100644 tests/python/core/synchronization/test_device_clock_estimator.py diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt index 1ab6f9c851..4a611b9ca8 100644 --- a/src/core/CMakeLists.txt +++ b/src/core/CMakeLists.txt @@ -9,6 +9,9 @@ add_subdirectory(schema) # Grades one binary schema against another; used by the conform test and by replay. add_subdirectory(schema_compat) +# Device-clock synchronization (no dependencies; used by plugins whose device keeps its own clock) +add_subdirectory(synchronization) + # Build OXR Utils first - header-only utilities (no dependencies) add_subdirectory(oxr_utils) diff --git a/src/core/python/CMakeLists.txt b/src/core/python/CMakeLists.txt index 361fd51c32..90769bbe5d 100644 --- a/src/core/python/CMakeLists.txt +++ b/src/core/python/CMakeLists.txt @@ -51,7 +51,7 @@ add_custom_target(python_package ALL COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_CURRENT_SOURCE_DIR}/requirements-retargeters-lite.txt" "${CMAKE_BINARY_DIR}/python_package/$/" COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_CURRENT_SOURCE_DIR}/requirements-grounding.txt" "${CMAKE_BINARY_DIR}/python_package/$/" COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_CURRENT_SOURCE_DIR}/requirements-wuji.txt" "${CMAKE_BINARY_DIR}/python_package/$/" - DEPENDS deviceio_trackers_py deviceio_session_py oxr_py plugin_manager_py schema_py isaacteleop_python cloudxr_python stage_generated_tracker_exports + DEPENDS deviceio_trackers_py deviceio_session_py oxr_py plugin_manager_py schema_py synchronization_py isaacteleop_python cloudxr_python stage_generated_tracker_exports COMMENT "Preparing Python package structure" ) diff --git a/src/core/synchronization/CMakeLists.txt b/src/core/synchronization/CMakeLists.txt new file mode 100644 index 0000000000..4168808915 --- /dev/null +++ b/src/core/synchronization/CMakeLists.txt @@ -0,0 +1,11 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20) + +# Build the device-clock synchronization C++ library +add_subdirectory(cpp) + +if(BUILD_PYTHON_BINDINGS) + add_subdirectory(python) +endif() diff --git a/src/core/synchronization/cpp/CMakeLists.txt b/src/core/synchronization/cpp/CMakeLists.txt new file mode 100644 index 0000000000..72ff5f3b82 --- /dev/null +++ b/src/core/synchronization/cpp/CMakeLists.txt @@ -0,0 +1,22 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20) + +find_package(Threads REQUIRED) + +# Synchronization - maps a device's own free-running clock onto the local common clock. +# Pure arithmetic over round-trip observations: no transport, no threads, no device knowledge, +# and therefore no dependencies beyond the standard library. +add_library(synchronization + device_clock_estimator.cpp +) + +target_link_libraries(synchronization PRIVATE Threads::Threads) + +target_include_directories(synchronization + INTERFACE + $ +) + +add_library(synchronization::synchronization ALIAS synchronization) diff --git a/src/core/synchronization/cpp/device_clock_estimator.cpp b/src/core/synchronization/cpp/device_clock_estimator.cpp new file mode 100644 index 0000000000..30603e9124 --- /dev/null +++ b/src/core/synchronization/cpp/device_clock_estimator.cpp @@ -0,0 +1,284 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/synchronization/device_clock_estimator.hpp" + +#include +#include +#include +#include + +namespace core +{ +namespace +{ + +// Fit the lowest-RTT quarter to limit delay-asymmetry bias. +constexpr double kKeepFraction = 0.25; + +// Reject RTTs above this multiple of the recent median. +constexpr double kRttRejectFactor = 3.0; + +// Maximum RTT history length. +constexpr size_t kRttHistorySize = 16; + +// Minimum observations required for calibration. +constexpr size_t kMinObservations = 8; + +// Accept this many RTTs before rejecting outliers. +constexpr size_t kMinRttReference = 4; + +double median_of(std::vector values) +{ + if (values.empty()) + { + return 0.0; + } + const size_t middle = values.size() / 2; + std::nth_element(values.begin(), values.begin() + static_cast(middle), values.end()); + return values[middle]; +} + +} // namespace + +ClockObservation make_observation(int64_t t1, int64_t t2, int64_t t3, int64_t t4) +{ + ClockObservation observation; + // Attribute the exchange to the midpoint of the device-side timestamps. + observation.device_ns = t2 + (t3 - t2) / 2; + observation.offset_ns = ((t2 - t1) + (t3 - t4)) / 2; + observation.rtt_ns = (t4 - t1) - (t3 - t2); + return observation; +} + +int64_t ClockCalibration::to_local_common_ns(int64_t device_ns) const +{ + // device = host + theta(t), theta(t) = a + b*(t - d0) + const double correction = a_ns + b * static_cast(device_ns - d0_ns); + return device_ns - std::llround(correction); +} + +DeviceClockEstimator::DeviceClockEstimator(Mode mode, double window_s, double min_span_s, double max_extrapolation_windows) + : mode_(mode), + window_ns_(static_cast(window_s * 1e9)), + min_span_ns_(static_cast(min_span_s * 1e9)), + max_extrapolation_windows_(max_extrapolation_windows) +{ + if (!std::isfinite(max_extrapolation_windows_) || max_extrapolation_windows_ <= 1.0) + { + throw std::invalid_argument("max_extrapolation_windows must be finite and greater than one"); + } + publish_snapshot(); +} + +bool DeviceClockEstimator::reject(int64_t rtt_ns) +{ + bool rejected = false; + if (rtt_history_.size() >= kMinRttReference) + { + const double median = median_of(std::vector(rtt_history_.begin(), rtt_history_.end())); + rejected = static_cast(rtt_ns) > kRttRejectFactor * median; + } + + rtt_history_.push_back(rtt_ns); + if (rtt_history_.size() > kRttHistorySize) + { + rtt_history_.pop_front(); + } + + if (rejected) + { + ++stats_.rejected; + } + return rejected; +} + +void DeviceClockEstimator::reset() +{ + window_.clear(); + rtt_history_.clear(); + calibration_ = ClockCalibration{}; + calibration_end_device_ns_ = 0; + last_observation_device_ns_.reset(); + ++stats_.resets; + stats_.skew_ppm = 0.0; + stats_.resid_ns = 0.0; + stats_.rtt_ns = 0.0; + stats_.span_s = 0.0; + stats_.n = 0; +} + +bool DeviceClockEstimator::update(const ClockObservation& observation) +{ + const std::lock_guard lock(update_mutex_); + + // Reset before RTT filtering so the previous epoch cannot reject the first new observation. + if (last_observation_device_ns_.has_value() && observation.device_ns < last_observation_device_ns_.value()) + { + reset(); + } + last_observation_device_ns_ = observation.device_ns; + + if (reject(observation.rtt_ns)) + { + publish_snapshot(); + return false; + } + + if (mode_ == Mode::Latest) + { + // Latest mode publishes the accepted observation directly. + calibration_ = ClockCalibration{ observation.device_ns, static_cast(observation.offset_ns), 0.0, true }; + calibration_end_device_ns_ = observation.device_ns; + stats_.skew_ppm = 0.0; + stats_.resid_ns = 0.0; + stats_.rtt_ns = static_cast(observation.rtt_ns); + stats_.span_s = 0.0; + stats_.n = 1; + publish_snapshot(); + return true; + } + + // Evict observations outside the device-time window. + window_.push_back(observation); + const int64_t cutoff = observation.device_ns - window_ns_; + while (!window_.empty() && window_.front().device_ns < cutoff) + { + window_.pop_front(); + } + + refit(); + publish_snapshot(); + return true; +} + +void DeviceClockEstimator::refit() +{ + stats_.n = window_.size(); + if (window_.size() < kMinObservations) + { + return; + } + + const int64_t span_ns = window_.back().device_ns - window_.front().device_ns; + + // Use a stable pivot before RTT selection reorders the observations. + const int64_t d0 = window_.front().device_ns; + + std::vector retained(window_.begin(), window_.end()); + const size_t keep = + std::max(kMinObservations, static_cast(static_cast(retained.size()) * kKeepFraction)); + if (keep < retained.size()) + { + std::nth_element(retained.begin(), retained.begin() + static_cast(keep), retained.end(), + [](const ClockObservation& lhs, const ClockObservation& rhs) + { return lhs.rtt_ns < rhs.rtt_ns; }); + retained.resize(keep); + } + + std::vector rtts; + rtts.reserve(retained.size()); + int64_t fit_end_device_ns = retained.front().device_ns; + for (const auto& observation : retained) + { + rtts.push_back(static_cast(observation.rtt_ns)); + fit_end_device_ns = std::max(fit_end_device_ns, observation.device_ns); + } + + // Below min_span_s the slope is noise rather than drift, so pin it and use the median offset. + if (span_ns < min_span_ns_) + { + std::vector offsets; + offsets.reserve(retained.size()); + for (const auto& observation : retained) + { + offsets.push_back(static_cast(observation.offset_ns)); + } + calibration_ = ClockCalibration{ d0, median_of(std::move(offsets)), 0.0, true }; + calibration_end_device_ns_ = fit_end_device_ns; + stats_.span_s = static_cast(span_ns) * 1e-9; + stats_.rtt_ns = median_of(std::move(rtts)); + stats_.skew_ppm = 0.0; + stats_.resid_ns = 0.0; + return; + } + + // Ordinary least squares: b = cov(x, y) / var(x), a = mean_y - b * mean_x. + double mean_x = 0.0; + double mean_y = 0.0; + for (const auto& observation : retained) + { + mean_x += static_cast(observation.device_ns - d0); + mean_y += static_cast(observation.offset_ns); + } + mean_x /= static_cast(retained.size()); + mean_y /= static_cast(retained.size()); + + double covariance = 0.0; + double variance = 0.0; + for (const auto& observation : retained) + { + const double dx = static_cast(observation.device_ns - d0) - mean_x; + covariance += dx * (static_cast(observation.offset_ns) - mean_y); + variance += dx * dx; + } + if (variance <= 0.0) + { + // Keep the previous calibration when no slope can be fitted. + return; + } + + const double b = covariance / variance; + const double a = mean_y - b * mean_x; + + std::vector residuals; + residuals.reserve(retained.size()); + for (const auto& observation : retained) + { + const double predicted = a + b * static_cast(observation.device_ns - d0); + residuals.push_back(std::abs(static_cast(observation.offset_ns) - predicted)); + } + + calibration_ = ClockCalibration{ d0, a, b, true }; + calibration_end_device_ns_ = fit_end_device_ns; + stats_.span_s = static_cast(span_ns) * 1e-9; + stats_.rtt_ns = median_of(std::move(rtts)); + stats_.skew_ppm = b * 1e6; + stats_.resid_ns = median_of(std::move(residuals)); +} + +ClockCalibration DeviceClockEstimator::calibration() const +{ + return snapshot_.load(std::memory_order_acquire)->calibration; +} + +ClockConversionResult DeviceClockEstimator::to_local_common_ns(int64_t device_ns) const +{ + const auto snapshot = snapshot_.load(std::memory_order_acquire); + if (!snapshot->calibration.valid) + { + return { ClockStatus::Uncalibrated, 0 }; + } + + const double extrapolation_ns = static_cast(device_ns - snapshot->calibration_end_device_ns); + const double limit_ns = static_cast(window_ns_) * max_extrapolation_windows_; + if (extrapolation_ns > limit_ns) + { + return { ClockStatus::Stale, 0 }; + } + + return { ClockStatus::Synchronized, snapshot->calibration.to_local_common_ns(device_ns) }; +} + +DeviceClockEstimator::Stats DeviceClockEstimator::stats() const +{ + return snapshot_.load(std::memory_order_acquire)->stats; +} + +void DeviceClockEstimator::publish_snapshot() +{ + auto snapshot = std::make_shared(Snapshot{ calibration_, stats_, calibration_end_device_ns_ }); + snapshot_.store(std::move(snapshot), std::memory_order_release); +} + +} // namespace core diff --git a/src/core/synchronization/cpp/inc/synchronization/device_clock_estimator.hpp b/src/core/synchronization/cpp/inc/synchronization/device_clock_estimator.hpp new file mode 100644 index 0000000000..a808789b67 --- /dev/null +++ b/src/core/synchronization/cpp/inc/synchronization/device_clock_estimator.hpp @@ -0,0 +1,210 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace core +{ + +/** + * @brief One reduced NTP-style clock exchange. + */ +struct ClockObservation +{ + //! Device clock at the exchange. + int64_t device_ns = 0; + + //! Device clock minus host clock. + int64_t offset_ns = 0; + + //! Round trip. The error on offset_ns is bounded by rtt_ns / 2. + int64_t rtt_ns = 0; +}; + +/** + * @brief Reduce the four NTP-style timestamps of one exchange to an observation. + * + * @param t1 Host transmit time. + * @param t2 Device receive time. + * @param t3 Device transmit time. + * @param t4 Host receive time. + * @return The reduced observation. + * + * The offset assumes equal one-way delays. Link asymmetry introduces an error bounded by rtt / 2. + */ +ClockObservation make_observation(int64_t t1, int64_t t2, int64_t t3, int64_t t4); + +/** + * @brief An affine map from a device's own clock to the local common clock. + */ +struct ClockCalibration +{ + //! Reference point on the device clock; the offset below is measured here. + int64_t d0_ns = 0; + + //! Offset in nanoseconds at d0_ns. + double a_ns = 0.0; + + //! Skew in nanoseconds per nanosecond. 1e-6 is one part per million. + double b = 0.0; + + //! False until enough measurements have arrived to map anything. + bool valid = false; + + /** + * @brief Map a device timestamp onto the local common clock without a freshness check. + * @param device_ns Timestamp in the device's own clock. + * @return The same instant expressed in the local common clock. + * + * Prefer DeviceClockEstimator::to_local_common_ns(), which also checks freshness. Link + * asymmetry limits absolute accuracy to roughly rtt / 2. + */ + int64_t to_local_common_ns(int64_t device_ns) const; +}; + +// Result state for a checked device-to-local-common-clock conversion. +enum class ClockStatus +{ + Synchronized, + Uncalibrated, + Stale, +}; + +// Result of converting a device timestamp through the estimator's current calibration. +struct ClockConversionResult +{ + ClockStatus status = ClockStatus::Uncalibrated; + int64_t local_common_ns = 0; +}; + +/** + * @brief Estimates an affine map from a device clock to the local common clock. + * + * Transport agnostic: feed it probe observations and convert each sample timestamp: + * + * DeviceClockEstimator estimator; + * estimator.update(make_observation(t1, t2, t3, t4)); + * int64_t sample_ns = core::os_monotonic_now_ns(); + * const auto converted = estimator.to_local_common_ns(device_ns); + * if (converted.status == ClockStatus::Synchronized) + * { + * sample_ns = converted.local_common_ns; + * } + * + * Thread-safe. Readers use an immutable snapshot and never wait for fitting. Concurrent update() + * calls are serialized internally. + */ +class DeviceClockEstimator +{ +public: + enum class Mode + { + // Use the newest accepted observation with zero skew. + Latest, + + // Fit offset and skew over a sliding window. + Linear, + }; + + /** + * @brief Diagnostics for the current calibration. + */ + struct Stats + { + // Zero in Latest mode, and while the window spans less than min_span_s. + double skew_ppm = 0.0; + + // Median absolute residual of the fit. + double resid_ns = 0.0; + + // Median round trip of the retained observations. + double rtt_ns = 0.0; + + // Span of retained observations. + double span_s = 0.0; + + // Observations currently retained. + size_t n = 0; + + // Observations discarded for excessive round trip, since construction. + size_t rejected = 0; + + // Device-clock restarts since construction. + size_t resets = 0; + }; + + /** + * @brief Construct an estimator. + * @param mode Latest or Linear. + * @param window_s Sliding-window length. + * @param min_span_s Span required before fitting skew. + * @param max_extrapolation_windows Freshness limit as a multiple of window_s; must exceed one. + */ + explicit DeviceClockEstimator(Mode mode = Mode::Linear, + double window_s = 300.0, + double min_span_s = 30.0, + double max_extrapolation_windows = 3.0); + + /** + * @brief Feed one measurement. + * @param observation The reduced exchange, from make_observation(). + * @return false if the observation was rejected as an outlier, in which case the previous + * calibration is kept. + * + * A backward device timestamp resets the estimator to a new clock epoch. + */ + bool update(const ClockObservation& observation); + + //! @brief The current device-to-host mapping. Check valid before using it. + ClockCalibration calibration() const; + + /** + * @brief Map a device timestamp when the current calibration still covers it. + * @param device_ns Timestamp in the device's clock. + * @return Synchronized with a mapped timestamp, Uncalibrated, or Stale. + * + * Freshness is measured from the newest observation supporting the published calibration. + */ + ClockConversionResult to_local_common_ns(int64_t device_ns) const; + + //! @brief Diagnostics for the current calibration. + Stats stats() const; + +private: + struct Snapshot + { + ClockCalibration calibration; + Stats stats; + int64_t calibration_end_device_ns = 0; + }; + + bool reject(int64_t rtt_ns); + void reset(); + void refit(); + void publish_snapshot(); + + Mode mode_; + int64_t window_ns_; + int64_t min_span_ns_; + double max_extrapolation_windows_; + int64_t calibration_end_device_ns_ = 0; + std::optional last_observation_device_ns_; + + std::deque window_; + std::deque rtt_history_; + ClockCalibration calibration_; + Stats stats_; + + std::mutex update_mutex_; + std::atomic> snapshot_; +}; + +} // namespace core diff --git a/src/core/synchronization/python/CMakeLists.txt b/src/core/synchronization/python/CMakeLists.txt new file mode 100644 index 0000000000..5dc7ccaa6d --- /dev/null +++ b/src/core/synchronization/python/CMakeLists.txt @@ -0,0 +1,17 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +pybind11_add_module(synchronization_py + synchronization_bindings.cpp +) + +target_link_libraries(synchronization_py + PRIVATE + synchronization::synchronization +) + +set_target_properties(synchronization_py PROPERTIES + OUTPUT_NAME "_synchronization" + # Use genex in output directory - CMake won't add another config subdirectory + LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/python_package/$/isaacteleop/synchronization" +) diff --git a/src/core/synchronization/python/synchronization_bindings.cpp b/src/core/synchronization/python/synchronization_bindings.cpp new file mode 100644 index 0000000000..888b1860f6 --- /dev/null +++ b/src/core/synchronization/python/synchronization_bindings.cpp @@ -0,0 +1,85 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include + +namespace py = pybind11; +using namespace core; + +PYBIND11_MODULE(_synchronization, m) +{ + m.doc() = + "Isaac Teleop device-clock synchronization bindings.\n" + "\n" + "The same estimator the C++ plugins use, so a fit tried from Python -- in a\n" + "notebook, or driven by a simulator against known ground truth -- behaves exactly\n" + "as it will in the plugin."; + + py::class_( + m, "ClockObservation", "One round trip, reduced to the three numbers the estimator needs.") + .def(py::init<>()) + .def_readwrite("device_ns", &ClockObservation::device_ns, "Device clock at the exchange.") + .def_readwrite("offset_ns", &ClockObservation::offset_ns, "Device clock minus host clock.") + .def_readwrite( + "rtt_ns", &ClockObservation::rtt_ns, "Round trip. The error on offset_ns is bounded by rtt_ns / 2."); + + m.def("make_observation", &make_observation, py::arg("t1"), py::arg("t2"), py::arg("t3"), py::arg("t4"), + "Reduce the four NTP-style timestamps of one exchange to an observation.\n" + "\n" + "t1 host transmit, t2 device receive, t3 device transmit, t4 host receive. The\n" + "reduction is exact only if the two legs take equal time; on a USB link they do not,\n" + "which leaves a systematic error bounded by rtt / 2 that more samples do not reduce."); + + py::enum_(m, "ClockStatus", "State of a checked device-clock conversion.") + .value("Synchronized", ClockStatus::Synchronized) + .value("Uncalibrated", ClockStatus::Uncalibrated) + .value("Stale", ClockStatus::Stale); + + py::class_(m, "ClockConversionResult", "Result of a checked clock conversion.") + .def_readonly("status", &ClockConversionResult::status) + .def_readonly("local_common_ns", &ClockConversionResult::local_common_ns); + + // Deliberately not constructible from Python: a ClockCalibration is only meaningful as the + // output of an estimator, and hand-built ones would silently produce plausible timestamps. + py::class_(m, "ClockCalibration", "An affine map from a device clock to the local common clock.") + .def_readonly("d0_ns", &ClockCalibration::d0_ns, "Reference point on the device clock.") + .def_readonly("a_ns", &ClockCalibration::a_ns, "Offset in nanoseconds at d0_ns.") + .def_readonly("b", &ClockCalibration::b, "Skew in nanoseconds per nanosecond; 1e-6 is one ppm.") + .def_readonly("valid", &ClockCalibration::valid, "False until enough measurements have arrived.") + .def("to_local_common_ns", &ClockCalibration::to_local_common_ns, py::arg("device_ns"), + "Map a device timestamp onto the local common clock."); + + py::class_(m, "DeviceClockEstimatorStats", "Diagnostics for the current calibration.") + .def_readonly("skew_ppm", &DeviceClockEstimator::Stats::skew_ppm) + .def_readonly("resid_ns", &DeviceClockEstimator::Stats::resid_ns) + .def_readonly("rtt_ns", &DeviceClockEstimator::Stats::rtt_ns) + .def_readonly("span_s", &DeviceClockEstimator::Stats::span_s) + .def_readonly("n", &DeviceClockEstimator::Stats::n) + .def_readonly("rejected", &DeviceClockEstimator::Stats::rejected) + .def_readonly("resets", &DeviceClockEstimator::Stats::resets, "Device-clock restarts seen since construction."); + + py::class_ estimator( + m, "DeviceClockEstimator", + "Maintains a ClockCalibration from a stream of round-trip observations.\n" + "\n" + "Thread-safe. Readers use immutable snapshots and do not wait for fitting."); + + py::enum_(estimator, "Mode") + .value("Latest", DeviceClockEstimator::Mode::Latest, "Newest acceptable observation; skew pinned to zero.") + .value("Linear", DeviceClockEstimator::Mode::Linear, "Offset and skew fitted over a sliding window.") + .export_values(); + + estimator + .def(py::init(), + py::arg("mode") = DeviceClockEstimator::Mode::Linear, py::arg("window_s") = 300.0, + py::arg("min_span_s") = 30.0, py::arg("max_extrapolation_windows") = 3.0) + .def("update", &DeviceClockEstimator::update, py::arg("observation"), + "Feed one measurement. False if it was rejected as an outlier, in which case the\n" + "previous calibration is kept.") + .def("calibration", &DeviceClockEstimator::calibration, "The current device-to-host mapping. Check valid first.") + .def("to_local_common_ns", &DeviceClockEstimator::to_local_common_ns, py::arg("device_ns"), + "Map a device timestamp when the current calibration is usable and not stale.") + .def("stats", &DeviceClockEstimator::stats, "Diagnostics for the current calibration."); +} diff --git a/src/python/isaacteleop/synchronization/__init__.py b/src/python/isaacteleop/synchronization/__init__.py new file mode 100644 index 0000000000..f85db7a922 --- /dev/null +++ b/src/python/isaacteleop/synchronization/__init__.py @@ -0,0 +1,31 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Isaac Teleop device-clock synchronization. + +Maps a device's own free-running clock onto the local common clock, so a sample can carry the +instant it was taken rather than the instant the host got round to reading it. + +These are the same estimator the C++ plugins use, so a fit tried here behaves exactly as it will +in the plugin. +""" + +from ._synchronization import ( + ClockCalibration, + ClockConversionResult, + DeviceClockEstimator, + DeviceClockEstimatorStats, + ClockObservation, + ClockStatus, + make_observation, +) + +__all__ = [ + "ClockCalibration", + "ClockConversionResult", + "DeviceClockEstimator", + "DeviceClockEstimatorStats", + "ClockObservation", + "ClockStatus", + "make_observation", +] diff --git a/tests/cpp/core/CMakeLists.txt b/tests/cpp/core/CMakeLists.txt index 79c958f895..0bb0932122 100644 --- a/tests/cpp/core/CMakeLists.txt +++ b/tests/cpp/core/CMakeLists.txt @@ -2,6 +2,7 @@ # SPDX-License-Identifier: Apache-2.0 add_subdirectory(schema) +add_subdirectory(synchronization) add_subdirectory(mcap) add_subdirectory(replay_deviceio_session) add_subdirectory(live_trackers) diff --git a/tests/cpp/core/synchronization/CMakeLists.txt b/tests/cpp/core/synchronization/CMakeLists.txt new file mode 100644 index 0000000000..216f278459 --- /dev/null +++ b/tests/cpp/core/synchronization/CMakeLists.txt @@ -0,0 +1,19 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20) + +find_package(Threads REQUIRED) + +add_executable(synchronization_tests + test_device_clock_estimator.cpp +) + +target_link_libraries(synchronization_tests PRIVATE + synchronization::synchronization + Catch2::Catch2WithMain + Threads::Threads +) + +message(STATUS "synchronization_tests target enabled with Catch2") +catch_discover_tests(synchronization_tests) diff --git a/tests/cpp/core/synchronization/test_device_clock_estimator.cpp b/tests/cpp/core/synchronization/test_device_clock_estimator.cpp new file mode 100644 index 0000000000..dac6d0ce35 --- /dev/null +++ b/tests/cpp/core/synchronization/test_device_clock_estimator.cpp @@ -0,0 +1,413 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Synthetic symmetric exchanges provide exact clock ground truth. + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace +{ + +constexpr int64_t kMs = 1'000'000; +constexpr int64_t kSecond = 1'000'000'000; + +// Build a symmetric exchange with known offset and skew. +core::ClockObservation exchange( + int64_t host_send_ns, int64_t offset_ns, int64_t rtt_ns, double skew = 0.0, int64_t host_epoch_ns = 0) +{ + const int64_t t1 = host_send_ns; + const int64_t t4 = t1 + rtt_ns; + const int64_t midpoint = t1 + rtt_ns / 2; + const int64_t drift = static_cast(std::llround(skew * static_cast(midpoint - host_epoch_ns))); + const int64_t device_at_midpoint = midpoint + offset_ns + drift; + // Instant device reply: t2 == t3. + return core::make_observation(t1, device_at_midpoint, device_at_midpoint, t4); +} + +// Feed evenly spaced exchanges and return the next host time. +int64_t feed(core::DeviceClockEstimator& estimator, + int64_t start_ns, + int count, + int64_t period_ns, + int64_t offset_ns, + int64_t rtt_ns, + double skew = 0.0) +{ + int64_t host_ns = start_ns; + for (int i = 0; i < count; ++i) + { + estimator.update(exchange(host_ns, offset_ns, rtt_ns, skew, start_ns)); + host_ns += period_ns; + } + return host_ns; +} + +} // namespace + +TEST_CASE("make_observation reduces the four timestamps", "[unit][synchronization]") +{ + // Device is 5 ms ahead at the 1200 ns exchange midpoint. + const core::ClockObservation observation = core::make_observation(1000, 1200 + 5 * kMs, 1200 + 5 * kMs, 1400); + + CHECK(observation.offset_ns == 5 * kMs); + CHECK(observation.rtt_ns == 400); + // Device-side midpoint. + CHECK(observation.device_ns == 1200 + 5 * kMs); +} + +TEST_CASE("make_observation keeps the sign of a device behind the host", "[unit][synchronization]") +{ + const core::ClockObservation observation = core::make_observation(1000, 1200 - 5 * kMs, 1200 - 5 * kMs, 1400); + CHECK(observation.offset_ns == -5 * kMs); +} + +TEST_CASE("an uncalibrated estimator maps nothing", "[unit][synchronization]") +{ + const core::DeviceClockEstimator estimator; + CHECK_FALSE(estimator.calibration().valid); + CHECK(estimator.stats().n == 0); + + const core::ClockConversionResult result = estimator.to_local_common_ns(123); + CHECK(result.status == core::ClockStatus::Uncalibrated); + CHECK(result.local_common_ns == 0); +} + +TEST_CASE("to_local_common_ns removes offset and skew", "[unit][synchronization]") +{ + core::ClockCalibration calibration; + calibration.d0_ns = 1000; + calibration.a_ns = 5.0 * static_cast(kMs); + calibration.b = 1e-5; + calibration.valid = true; + + // At the reference point only the constant applies. + CHECK(calibration.to_local_common_ns(1000) == 1000 - 5 * kMs); + // One second later the skew has added another 10 us of device time. + CHECK(calibration.to_local_common_ns(1000 + kSecond) == 1000 + kSecond - 5 * kMs - 10'000); +} + +TEST_CASE("the map is strictly increasing, so samples cannot be reordered", "[unit][synchronization]") +{ + core::ClockCalibration calibration; + calibration.b = 1e-4; + calibration.valid = true; + + int64_t previous = calibration.to_local_common_ns(0); + for (int64_t device_ns = kMs; device_ns <= 100 * kMs; device_ns += kMs) + { + const int64_t mapped = calibration.to_local_common_ns(device_ns); + CHECK(mapped > previous); + previous = mapped; + } +} + +TEST_CASE("Latest mode takes the newest observation as the calibration", "[unit][synchronization]") +{ + core::DeviceClockEstimator estimator(core::DeviceClockEstimator::Mode::Latest); + + REQUIRE(estimator.update(exchange(0, 7 * kMs, 400))); + + const core::ClockCalibration calibration = estimator.calibration(); + REQUIRE(calibration.valid); + CHECK(calibration.a_ns == static_cast(7 * kMs)); + CHECK(calibration.b == 0.0); + CHECK(estimator.stats().n == 1); + CHECK(estimator.stats().skew_ppm == 0.0); +} + +TEST_CASE("Latest mode applies the same extrapolation boundary", "[unit][synchronization]") +{ + constexpr int64_t kWindowNs = 10 * kSecond; + constexpr int64_t kLimitNs = 2 * kWindowNs; + core::DeviceClockEstimator estimator(core::DeviceClockEstimator::Mode::Latest, 10.0, 1.0, 2.0); + const core::ClockObservation observation = exchange(0, 7 * kMs, 400); + REQUIRE(estimator.update(observation)); + + CHECK(estimator.to_local_common_ns(observation.device_ns + kLimitNs).status == core::ClockStatus::Synchronized); + CHECK(estimator.to_local_common_ns(observation.device_ns + kLimitNs + 1).status == core::ClockStatus::Stale); +} + +TEST_CASE("Latest mode detects a device clock restart", "[unit][synchronization]") +{ + core::DeviceClockEstimator estimator(core::DeviceClockEstimator::Mode::Latest); + for (int i = 0; i < 4; ++i) + { + REQUIRE(estimator.update(exchange(i * kSecond, 3 * kMs, 400))); + } + + core::ClockObservation restarted; + restarted.device_ns = 1000; + restarted.offset_ns = -4 * kSecond; + restarted.rtt_ns = 400; + REQUIRE(estimator.update(restarted)); + + CHECK(estimator.stats().resets == 1); + CHECK(estimator.calibration().d0_ns == restarted.device_ns); + CHECK(estimator.to_local_common_ns(restarted.device_ns).status == core::ClockStatus::Synchronized); +} + +TEST_CASE("Linear mode recovers a known offset and skew", "[unit][synchronization]") +{ + core::DeviceClockEstimator estimator(core::DeviceClockEstimator::Mode::Linear, 300.0, 30.0); + + // 60 s of exchanges at 1 Hz: past min_span_s, so the slope is fitted rather than pinned. + feed(estimator, 0, 60, kSecond, 3 * kMs, 400, 11.9e-6); + + const core::ClockCalibration calibration = estimator.calibration(); + REQUIRE(calibration.valid); + CHECK(estimator.stats().skew_ppm == Catch::Approx(11.9).margin(0.5)); + // The device reference point maps back to its host instant. + CHECK(calibration.to_local_common_ns(calibration.d0_ns) == + Catch::Approx(static_cast(calibration.d0_ns - 3 * kMs)).margin(2000.0)); +} + +TEST_CASE("a window shorter than min_span_s pins the slope", "[unit][synchronization]") +{ + core::DeviceClockEstimator estimator(core::DeviceClockEstimator::Mode::Linear, 300.0, 30.0); + + // 10 s of exchanges: enough observations to fit, too short a span to trust a slope. + feed(estimator, 0, 20, 500 * kMs, 3 * kMs, 400, 11.9e-6); + + REQUIRE(estimator.calibration().valid); + CHECK(estimator.calibration().b == 0.0); + CHECK(estimator.stats().skew_ppm == 0.0); +} + +TEST_CASE("a slow exchange is rejected and leaves the calibration alone", "[unit][synchronization]") +{ + core::DeviceClockEstimator estimator(core::DeviceClockEstimator::Mode::Linear, 300.0, 30.0); + + const int64_t host_ns = feed(estimator, 0, 40, kSecond, 3 * kMs, 400, 11.9e-6); + const core::ClockCalibration before = estimator.calibration(); + REQUIRE(before.valid); + + // Ten times the usual round trip: past kRttRejectFactor, so it never reaches the fit. + CHECK_FALSE(estimator.update(exchange(host_ns, 3 * kMs, 4000, 11.9e-6))); + + CHECK(estimator.stats().rejected == 1); + CHECK(estimator.calibration().a_ns == before.a_ns); + CHECK(estimator.calibration().b == before.b); +} + +TEST_CASE("a device clock restart discards the stale epoch", "[unit][synchronization]") +{ + core::DeviceClockEstimator estimator(core::DeviceClockEstimator::Mode::Linear, 300.0, 30.0); + + feed(estimator, 0, 60, kSecond, 3 * kMs, 400, 11.9e-6); + REQUIRE(estimator.calibration().valid); + REQUIRE(estimator.stats().resets == 0); + + // Device clock restarts while the host clock continues. + core::ClockObservation restarted; + restarted.device_ns = 1000; + restarted.offset_ns = -60 * kSecond; + restarted.rtt_ns = 400; + estimator.update(restarted); + + CHECK(estimator.stats().resets == 1); + CHECK_FALSE(estimator.calibration().valid); + CHECK(estimator.stats().n <= 1); + CHECK(estimator.to_local_common_ns(restarted.device_ns).status == core::ClockStatus::Uncalibrated); +} + +TEST_CASE("the calibration recovers after a restart", "[unit][synchronization]") +{ + core::DeviceClockEstimator estimator(core::DeviceClockEstimator::Mode::Linear, 300.0, 30.0); + + feed(estimator, 0, 60, kSecond, 3 * kMs, 400, 11.9e-6); + + core::ClockObservation restarted; + restarted.device_ns = 1000; + restarted.offset_ns = -60 * kSecond; + restarted.rtt_ns = 400; + estimator.update(restarted); + REQUIRE_FALSE(estimator.calibration().valid); + + // Fresh exchanges against the new epoch: the device now reads 2 ms behind the host. + feed(estimator, 100 * kSecond, 60, kSecond, -2 * kMs, 400); + + const core::ClockCalibration calibration = estimator.calibration(); + REQUIRE(calibration.valid); + CHECK(calibration.to_local_common_ns(calibration.d0_ns) == + Catch::Approx(static_cast(calibration.d0_ns + 2 * kMs)).margin(2000.0)); +} + +TEST_CASE("too few observations leave the calibration invalid", "[unit][synchronization]") +{ + core::DeviceClockEstimator estimator(core::DeviceClockEstimator::Mode::Linear, 300.0, 30.0); + + // One fewer than the minimum calibration sample count. + feed(estimator, 0, 7, kSecond, 3 * kMs, 400); + + CHECK_FALSE(estimator.calibration().valid); + CHECK(estimator.stats().n == 7); +} + +TEST_CASE("a zero-length window never calibrates rather than calibrating wrongly", "[unit][synchronization]") +{ + // A misconfigured window evicts every earlier observation, so the fit can never see enough. + core::DeviceClockEstimator estimator(core::DeviceClockEstimator::Mode::Linear, 0.0, 30.0); + + feed(estimator, 0, 60, kSecond, 3 * kMs, 400, 11.9e-6); + + CHECK_FALSE(estimator.calibration().valid); + CHECK(estimator.stats().n <= 1); +} + +TEST_CASE("a device running slow is recovered with the opposite sign", "[unit][synchronization]") +{ + core::DeviceClockEstimator estimator(core::DeviceClockEstimator::Mode::Linear, 300.0, 30.0); + + // Every other test uses a device running fast; the sign has to survive the fit and the map. + feed(estimator, 0, 60, kSecond, -4 * kMs, 400, -11.9e-6); + + const core::ClockCalibration calibration = estimator.calibration(); + REQUIRE(calibration.valid); + CHECK(estimator.stats().skew_ppm == Catch::Approx(-11.9).margin(0.5)); + CHECK(calibration.to_local_common_ns(calibration.d0_ns) == + Catch::Approx(static_cast(calibration.d0_ns + 4 * kMs)).margin(2000.0)); +} + +TEST_CASE("the window stops growing once it is full", "[unit][synchronization]") +{ + core::DeviceClockEstimator estimator(core::DeviceClockEstimator::Mode::Linear, 30.0, 5.0); + + // At 1 Hz, an inclusive 30 s window retains 31 observations. + int64_t host_ns = feed(estimator, 0, 45, kSecond, 3 * kMs, 400, 11.9e-6); + const size_t filled = estimator.stats().n; + CHECK(estimator.stats().span_s == Catch::Approx(30.0).margin(1.5)); + + // Another window's worth must hold both the count and the span steady. + feed(estimator, host_ns, 45, kSecond, 3 * kMs, 400, 11.9e-6); + + CHECK(estimator.stats().n == filled); + CHECK(estimator.stats().span_s == Catch::Approx(30.0).margin(1.5)); +} + +TEST_CASE("a rejected outlier does not poison the next observation", "[unit][synchronization]") +{ + core::DeviceClockEstimator estimator(core::DeviceClockEstimator::Mode::Linear, 300.0, 30.0); + + int64_t host_ns = feed(estimator, 0, 40, kSecond, 3 * kMs, 400, 11.9e-6); + REQUIRE_FALSE(estimator.update(exchange(host_ns, 3 * kMs, 4000, 11.9e-6))); + host_ns += kSecond; + + // A rejected RTT in the history must not reject the next normal RTT. + CHECK(estimator.update(exchange(host_ns, 3 * kMs, 400, 11.9e-6))); + CHECK(estimator.stats().rejected == 1); +} + +TEST_CASE("checked conversion expires after several fitting windows", "[unit][synchronization]") +{ + constexpr double kWindowS = 10.0; + constexpr double kExtrapolationWindows = 3.0; + core::DeviceClockEstimator estimator(core::DeviceClockEstimator::Mode::Linear, kWindowS, 5.0, kExtrapolationWindows); + + core::ClockObservation last; + for (int i = 0; i < 8; ++i) + { + last = exchange(i * kSecond, 3 * kMs, 400); + REQUIRE(estimator.update(last)); + } + REQUIRE(estimator.calibration().valid); + + const int64_t limit_ns = static_cast(kWindowS * kExtrapolationWindows * 1e9); + const core::ClockConversionResult at_limit = estimator.to_local_common_ns(last.device_ns + limit_ns); + CHECK(at_limit.status == core::ClockStatus::Synchronized); + CHECK(at_limit.local_common_ns == estimator.calibration().to_local_common_ns(last.device_ns + limit_ns)); + + const core::ClockConversionResult beyond_limit = estimator.to_local_common_ns(last.device_ns + limit_ns + 1); + CHECK(beyond_limit.status == core::ClockStatus::Stale); + CHECK(beyond_limit.local_common_ns == 0); +} + +TEST_CASE("a rejected observation does not refresh calibration freshness", "[unit][synchronization]") +{ + core::DeviceClockEstimator estimator(core::DeviceClockEstimator::Mode::Linear, 10.0, 5.0, 3.0); + + for (int i = 0; i < 8; ++i) + { + REQUIRE(estimator.update(exchange(i * kSecond, 3 * kMs, 400))); + } + REQUIRE(estimator.calibration().valid); + + const core::ClockObservation delayed = exchange(100 * kSecond, 3 * kMs, 4000); + REQUIRE_FALSE(estimator.update(delayed)); + CHECK(estimator.to_local_common_ns(delayed.device_ns).status == core::ClockStatus::Stale); +} + +TEST_CASE("an accepted observation without a new fit does not refresh calibration freshness", "[unit][synchronization]") +{ + core::DeviceClockEstimator estimator(core::DeviceClockEstimator::Mode::Linear, 10.0, 5.0, 3.0); + + for (int i = 0; i < 8; ++i) + { + REQUIRE(estimator.update(exchange(i * kSecond, 3 * kMs, 400))); + } + REQUIRE(estimator.calibration().valid); + + core::ClockObservation latest = exchange(100 * kSecond, 3 * kMs, 400); + REQUIRE(estimator.update(latest)); + REQUIRE(estimator.stats().n == 1); + CHECK(estimator.to_local_common_ns(latest.device_ns).status == core::ClockStatus::Stale); + + for (int i = 1; i < 8; ++i) + { + latest = exchange((100 + i) * kSecond, 3 * kMs, 400); + REQUIRE(estimator.update(latest)); + } + REQUIRE(estimator.stats().n == 8); + CHECK(estimator.to_local_common_ns(latest.device_ns).status == core::ClockStatus::Synchronized); +} + +TEST_CASE("extrapolation limit must exceed one fitting window", "[unit][synchronization]") +{ + CHECK_THROWS_AS( + core::DeviceClockEstimator(core::DeviceClockEstimator::Mode::Linear, 10.0, 5.0, 1.0), std::invalid_argument); +} + +TEST_CASE("readers see complete calibration snapshots", "[unit][synchronization][threading]") +{ + core::DeviceClockEstimator estimator(core::DeviceClockEstimator::Mode::Latest); + core::ClockObservation observation{ 1, kMs, 400 }; + REQUIRE(estimator.update(observation)); + + constexpr int64_t kQueryDeviceNs = 10 * kMs; + std::atomic valid{ true }; + std::thread reader( + [&] + { + for (int i = 0; i < 20'000; ++i) + { + const auto result = estimator.to_local_common_ns(kQueryDeviceNs); + const bool complete = result.status == core::ClockStatus::Synchronized && + (result.local_common_ns == kQueryDeviceNs - kMs || + result.local_common_ns == kQueryDeviceNs - 2 * kMs); + if (!complete) + { + valid.store(false, std::memory_order_relaxed); + return; + } + } + }); + + bool updates_valid = true; + for (int64_t device_ns = 2; device_ns < 2'000; ++device_ns) + { + observation.device_ns = device_ns; + observation.offset_ns = device_ns % 2 == 0 ? 2 * kMs : kMs; + updates_valid = estimator.update(observation) && updates_valid; + } + + reader.join(); + CHECK(updates_valid); + CHECK(valid.load(std::memory_order_relaxed)); +} diff --git a/tests/python/core/CMakeLists.txt b/tests/python/core/CMakeLists.txt index eb680148f9..9f73984f1b 100644 --- a/tests/python/core/CMakeLists.txt +++ b/tests/python/core/CMakeLists.txt @@ -8,4 +8,5 @@ add_subdirectory(teleop_session_manager) if(BUILD_PYTHON_BINDINGS) add_subdirectory(cloudxr) add_subdirectory(rig) + add_subdirectory(synchronization) endif() diff --git a/tests/python/core/synchronization/CMakeLists.txt b/tests/python/core/synchronization/CMakeLists.txt new file mode 100644 index 0000000000..044381aa4f --- /dev/null +++ b/tests/python/core/synchronization/CMakeLists.txt @@ -0,0 +1,29 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# ============================================================================== +# Synchronization Python Tests +# ============================================================================== + +# One CTest entry per test file, so a failure names the file it came from. +# CONFIGURE_DEPENDS: a new test_*.py registers without a manual reconfigure. +file(GLOB TEST_FILES + RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" + CONFIGURE_DEPENDS + "${CMAKE_CURRENT_SOURCE_DIR}/test_*.py" +) + +foreach(test_file ${TEST_FILES}) + get_filename_component(test_name "${test_file}" NAME_WE) + + add_test( + NAME "synchronization_${test_name}" + COMMAND uv run --python ${ISAAC_TELEOP_PYTHON_VERSION} --extra dev pytest -v --tb=short "${test_file}" + WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" + ) + + # The bindings are only importable from the staged package tree. + set_tests_properties("synchronization_${test_name}" PROPERTIES + ENVIRONMENT "PYTHONPATH=${CMAKE_BINARY_DIR}/python_package/$" + ) +endforeach() diff --git a/tests/python/core/synchronization/pyproject.toml b/tests/python/core/synchronization/pyproject.toml new file mode 100644 index 0000000000..d3d5426a78 --- /dev/null +++ b/tests/python/core/synchronization/pyproject.toml @@ -0,0 +1,23 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +[project] +name = "teleopcore-synchronization-tests" +version = "0.0.0" # Internal tests - not versioned +description = "Device-clock synchronization binding tests for TeleopCore" +requires-python = ">=3.11,<3.14" + +[project.optional-dependencies] +# numpy is not used by these tests directly: importing isaacteleop pulls in the +# retargeting engine, which needs it. Same reason tests/python/core/schema lists it. +dev = [ + "pytest", + "numpy", +] + +[tool.pytest.ini_options] +python_files = ["test_*.py"] +python_classes = ["Test*"] +python_functions = ["test_*"] +# Prevent pytest from recursing into parent directories +norecursedirs = [".git", ".venv", "build", "dist", "*.egg", "__pycache__"] diff --git a/tests/python/core/synchronization/test_device_clock_estimator.py b/tests/python/core/synchronization/test_device_clock_estimator.py new file mode 100644 index 0000000000..0c7980976e --- /dev/null +++ b/tests/python/core/synchronization/test_device_clock_estimator.py @@ -0,0 +1,149 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Test the Python synchronization API; C++ tests cover estimator arithmetic.""" + +import pytest +from isaacteleop.synchronization import ( + ClockCalibration, + ClockConversionResult, + DeviceClockEstimator, + DeviceClockEstimatorStats, + ClockObservation, + ClockStatus, + make_observation, +) + +MS = 1_000_000 +SECOND = 1_000_000_000 + + +def exchange(host_send_ns, offset_ns, rtt_ns=400, skew=0.0, epoch_ns=0): + """One symmetric exchange against a device `offset_ns` ahead and running `skew` fast.""" + t1 = host_send_ns + t4 = t1 + rtt_ns + midpoint = t1 + rtt_ns // 2 + device = midpoint + offset_ns + round(skew * (midpoint - epoch_ns)) + # The device replies instantly, so t2 == t3. + return make_observation(t1, device, device, t4) + + +def feed(estimator, count, period_ns, offset_ns, skew=0.0, start_ns=0): + host_ns = start_ns + for _ in range(count): + estimator.update(exchange(host_ns, offset_ns, skew=skew, epoch_ns=start_ns)) + host_ns += period_ns + return host_ns + + +def test_make_observation_is_bound_and_returns_readable_fields(): + observation = make_observation(1000, 1200 + 5 * MS, 1200 + 5 * MS, 1400) + + assert isinstance(observation, ClockObservation) + # Check that fields cross the binding boundary. + assert isinstance(observation.offset_ns, int) + assert isinstance(observation.rtt_ns, int) + assert isinstance(observation.device_ns, int) + + +def test_observation_is_constructible_and_writable(): + observation = ClockObservation() + assert observation.device_ns == 0 + + observation.device_ns = 7 + observation.offset_ns = -3 + observation.rtt_ns = 11 + assert (observation.device_ns, observation.offset_ns, observation.rtt_ns) == ( + 7, + -3, + 11, + ) + + +def test_calibration_cannot_be_constructed_from_python(): + # Calibrations must come from the estimator. + with pytest.raises(TypeError): + ClockCalibration() + + +def test_calibration_fields_are_read_only(): + estimator = DeviceClockEstimator(DeviceClockEstimator.Mode.Latest) + estimator.update(exchange(0, 5 * MS)) + calibration = estimator.calibration() + + assert calibration.valid + with pytest.raises(AttributeError): + calibration.a_ns = 0.0 + + +def test_estimator_defaults_survive_the_boundary(): + # The default 30 s minimum span pins skew during this 10 s run. + estimator = DeviceClockEstimator() + feed(estimator, count=20, period_ns=500 * MS, offset_ns=3 * MS, skew=11.9e-6) + + assert estimator.calibration().valid + assert estimator.calibration().b == 0.0 + + +def test_mode_enum_round_trips(): + assert DeviceClockEstimator.Mode.Latest != DeviceClockEstimator.Mode.Linear + estimator = DeviceClockEstimator( + mode=DeviceClockEstimator.Mode.Latest, window_s=10.0, min_span_s=1.0 + ) + assert estimator.update(exchange(0, 5 * MS)) + assert estimator.calibration().a_ns == pytest.approx(5 * MS) + + +def test_update_reports_rejection(): + estimator = DeviceClockEstimator() + host_ns = feed( + estimator, count=40, period_ns=SECOND, offset_ns=3 * MS, skew=11.9e-6 + ) + + # Ten times the usual round trip, so it is rejected rather than fitted. + assert estimator.update(exchange(host_ns, 3 * MS, rtt_ns=4000)) is False + assert estimator.stats().rejected == 1 + + +def test_stats_exposes_every_diagnostic(): + estimator = DeviceClockEstimator() + feed(estimator, count=60, period_ns=SECOND, offset_ns=3 * MS, skew=11.9e-6) + stats = estimator.stats() + + assert isinstance(stats, DeviceClockEstimatorStats) + assert stats.skew_ppm == pytest.approx(11.9, abs=0.5) + assert stats.n > 0 + assert stats.span_s > 0.0 + assert stats.rtt_ns > 0.0 + assert stats.resid_ns >= 0.0 + assert stats.rejected == 0 + assert stats.resets == 0 + + +def test_checked_conversion_result_and_status_round_trip(): + estimator = DeviceClockEstimator( + mode=DeviceClockEstimator.Mode.Latest, + window_s=10.0, + min_span_s=1.0, + max_extrapolation_windows=2.0, + ) + + uncalibrated = estimator.to_local_common_ns(123) + assert isinstance(uncalibrated, ClockConversionResult) + assert uncalibrated.status == ClockStatus.Uncalibrated + assert uncalibrated.local_common_ns == 0 + + observation = exchange(0, 5 * MS) + assert estimator.update(observation) + synchronized = estimator.to_local_common_ns(observation.device_ns) + assert synchronized.status == ClockStatus.Synchronized + assert isinstance(synchronized.local_common_ns, int) + + stale = estimator.to_local_common_ns(observation.device_ns + 20 * SECOND + 1) + assert stale.status == ClockStatus.Stale + assert stale.local_common_ns == 0 + + +def test_extrapolation_limit_must_exceed_one_window(): + with pytest.raises(ValueError, match="greater than one"): + DeviceClockEstimator(max_extrapolation_windows=1.0) From 6981672b110a2101200e991b385a4014764ffa71 Mon Sep 17 00:00:00 2001 From: Xinghua Sun Date: Wed, 9 Sep 2026 13:20:11 -0700 Subject: [PATCH 2/2] Add Python and C++ clock synchronization examples Signed-off-by: Xinghua Sun --- CMakeLists.txt | 1 + examples/synchronization/CMakeLists.txt | 9 ++ examples/synchronization/README.md | 69 ++++++++++++ examples/synchronization/cpp/CMakeLists.txt | 11 ++ .../cpp/synchronization_example.cpp | 105 ++++++++++++++++++ .../synchronization/python/pyproject.toml | 9 ++ .../python/synchronization_example.py | 94 ++++++++++++++++ 7 files changed, 298 insertions(+) create mode 100644 examples/synchronization/CMakeLists.txt create mode 100644 examples/synchronization/README.md create mode 100644 examples/synchronization/cpp/CMakeLists.txt create mode 100644 examples/synchronization/cpp/synchronization_example.cpp create mode 100644 examples/synchronization/python/pyproject.toml create mode 100644 examples/synchronization/python/synchronization_example.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 0dcde99227..ff8e2d531e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -171,6 +171,7 @@ if(BUILD_EXAMPLES) add_subdirectory(examples/mcap_record_replay) add_subdirectory(examples/deviceio_live_view) add_subdirectory(examples/haptic_feedback) + add_subdirectory(examples/synchronization) if(BUILD_VIZ) add_subdirectory(examples/mujoco_xr) endif() diff --git a/examples/synchronization/CMakeLists.txt b/examples/synchronization/CMakeLists.txt new file mode 100644 index 0000000000..3e7eb87dd3 --- /dev/null +++ b/examples/synchronization/CMakeLists.txt @@ -0,0 +1,9 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20) + +add_subdirectory(cpp) + +include(${CMAKE_SOURCE_DIR}/cmake/InstallPythonExample.cmake) +install_python_example(DESTINATION examples/synchronization/python) diff --git a/examples/synchronization/README.md b/examples/synchronization/README.md new file mode 100644 index 0000000000..78eb941967 --- /dev/null +++ b/examples/synchronization/README.md @@ -0,0 +1,69 @@ + + +# Device clock synchronization + +Demonstrates `DeviceClockEstimator` without requiring hardware or a particular transport. A +simulated device clock runs with a known offset and skew while synthetic NTP-style probes calibrate +it against the host clock. + +The goal is to estimate the relationship between the device clock and the host's common clock, then +convert capture times into that common clock domain. Multiple device streams can then be aligned by +their converted timestamps. + +## Requirements + +- The device must support timestamp echo: for each probe, it returns its device receive (`t2`) and + send (`t3`) timestamps plus an identifier that matches the reply to the request. +- Probe and sample timestamps must use the same free-running monotonic device clock. +- The host records send (`t1`) and receive (`t4`) times with one monotonic host clock, converts all + four timestamps to nanoseconds, and periodically feeds completed exchanges to the estimator. + +## How Synchronization Works + +Within a short sliding window, the monotonic device and host clocks are assumed to have an +approximately linear relationship. Probe and sample timestamps must use the same device clock. +Each probe records host send (`t1`), device receive (`t2`), device send (`t3`), and host receive +(`t4`). Assuming approximately symmetric link delay, `make_observation()` estimates the clock +offset and round-trip time (RTT). + +In linear mode, the estimator rejects unusually slow exchanges and retains observations within a +sliding device-time window. Once the retained span reaches `min_span_s`, it fits the offset model + +```text +offset(d) = a + b * (d - d0) +host_time = d - offset(d) +``` + +where `a` is the offset and `b` is the clock skew. Before enough time has elapsed to estimate skew, +`b` remains zero. Probe asymmetry contributes an offset error of at most roughly half the RTT. +`to_local_common_ns()` applies the map and returns `Synchronized`, `Uncalibrated`, or `Stale`. +A conversion becomes stale when it extrapolates beyond +`max_extrapolation_windows * window_s` from the newest observation supporting the fit. A backward +device timestamp is treated as a device restart and clears the old calibration. + +The estimator performs no I/O and creates no thread. If probe I/O can block, run it on a background +thread and call `update()` there. The sample thread can call `to_local_common_ns()` directly; it +reads an immutable atomic snapshot and never waits for a fit. A separate thread is unnecessary when +the caller already performs probes without blocking its sampling loop. + +Python: + +```bash +PYTHONPATH=build/python_package/Release \ + build/teleop_build_venv/bin/python \ + examples/synchronization/python/synchronization_example.py +``` + +C++: + +```bash +cmake --build build --target synchronization_example +./build/examples/synchronization/cpp/synchronization_example +``` + +Applications provide the four timestamps from each probe exchange and use the checked conversion +for every device sample. When conversion is uncalibrated or stale, use the sample's host arrival +time instead. diff --git a/examples/synchronization/cpp/CMakeLists.txt b/examples/synchronization/cpp/CMakeLists.txt new file mode 100644 index 0000000000..c70ceee4fa --- /dev/null +++ b/examples/synchronization/cpp/CMakeLists.txt @@ -0,0 +1,11 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20) + +add_executable(synchronization_example synchronization_example.cpp) +target_link_libraries(synchronization_example PRIVATE synchronization::synchronization) + +install(TARGETS synchronization_example + RUNTIME DESTINATION examples/synchronization/cpp +) diff --git a/examples/synchronization/cpp/synchronization_example.cpp b/examples/synchronization/cpp/synchronization_example.cpp new file mode 100644 index 0000000000..9a6c1533da --- /dev/null +++ b/examples/synchronization/cpp/synchronization_example.cpp @@ -0,0 +1,105 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include + +#include +#include +#include +#include +#include +#include + +namespace +{ + +constexpr int64_t kNsPerSecond = 1'000'000'000; + +struct SimulatedClock +{ + int64_t epoch_host_ns; + int64_t offset_ns; + double skew_ppm; + + int64_t device_ns(int64_t host_ns) const + { + const double rate = 1.0 + skew_ppm * 1e-6; + return offset_ns + static_cast(std::llround(static_cast(host_ns - epoch_host_ns) * rate)); + } +}; + +core::ClockObservation probe(const SimulatedClock& clock, int64_t host_send_ns) +{ + const int64_t device_receive_host_ns = host_send_ns + 80'000; + const int64_t device_send_host_ns = device_receive_host_ns + 20'000; + const int64_t host_receive_ns = device_send_host_ns + 120'000; + return core::make_observation( + host_send_ns, clock.device_ns(device_receive_host_ns), clock.device_ns(device_send_host_ns), host_receive_ns); +} + +void add_probes(core::DeviceClockEstimator& estimator, const SimulatedClock& clock, int64_t start_ns) +{ + for (int index = 0; index < 12; ++index) + { + estimator.update(probe(clock, start_ns + index * kNsPerSecond)); + } +} + +std::string_view status_name(core::ClockStatus status) +{ + switch (status) + { + case core::ClockStatus::Synchronized: + return "synchronized"; + case core::ClockStatus::Uncalibrated: + return "uncalibrated"; + case core::ClockStatus::Stale: + return "stale"; + } + return "unknown"; +} + +double conversion_error_us(const core::DeviceClockEstimator& estimator, const SimulatedClock& clock, int64_t host_ns) +{ + const auto converted = estimator.to_local_common_ns(clock.device_ns(host_ns)); + if (converted.status != core::ClockStatus::Synchronized) + { + throw std::runtime_error("expected a synchronized clock"); + } + return static_cast(converted.local_common_ns - host_ns) / 1'000.0; +} + +} // namespace + +int main() +{ + const int64_t start_ns = 1'000 * kNsPerSecond; + const SimulatedClock clock{ start_ns, 5 * kNsPerSecond, 20.0 }; + core::DeviceClockEstimator estimator(core::DeviceClockEstimator::Mode::Linear, 30.0, 5.0, 2.0); + + add_probes(estimator, clock, start_ns); + auto stats = estimator.stats(); + std::cout << std::fixed << std::setprecision(2) << "calibrated clock\n" + << " estimated skew: " << std::showpos << stats.skew_ppm << std::noshowpos << " ppm\n" + << " median RTT: " << stats.rtt_ns / 1'000.0 << " us\n" + << " conversion error: " << std::showpos + << conversion_error_us(estimator, clock, start_ns + 11 * kNsPerSecond) << std::noshowpos << " us\n"; + + const int64_t stale_host_ns = start_ns + 72 * kNsPerSecond; + const auto stale = estimator.to_local_common_ns(clock.device_ns(stale_host_ns)); + std::cout << "after probe silence: " << status_name(stale.status) << '\n'; + + const SimulatedClock restarted{ stale_host_ns, 100'000'000, -15.0 }; + estimator.update(probe(restarted, stale_host_ns)); + const auto restarting = estimator.to_local_common_ns(restarted.device_ns(stale_host_ns)); + std::cout << "after restart: " << status_name(restarting.status) << '\n'; + + add_probes(estimator, restarted, stale_host_ns + kNsPerSecond); + stats = estimator.stats(); + std::cout << "recalibrated clock\n" + << " resets: " << stats.resets << '\n' + << " estimated skew: " << std::showpos << stats.skew_ppm << std::noshowpos << " ppm\n" + << " conversion error: " << std::showpos + << conversion_error_us(estimator, restarted, stale_host_ns + 12 * kNsPerSecond) << std::noshowpos + << " us\n"; +} diff --git a/examples/synchronization/python/pyproject.toml b/examples/synchronization/python/pyproject.toml new file mode 100644 index 0000000000..8434f80311 --- /dev/null +++ b/examples/synchronization/python/pyproject.toml @@ -0,0 +1,9 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +[project] +name = "synchronization-example" +version = "0.0.0" +description = "Isaac Teleop device-clock synchronization example" +requires-python = ">=3.11,<3.14" +dependencies = ["isaacteleop"] diff --git a/examples/synchronization/python/synchronization_example.py b/examples/synchronization/python/synchronization_example.py new file mode 100644 index 0000000000..a550396f89 --- /dev/null +++ b/examples/synchronization/python/synchronization_example.py @@ -0,0 +1,94 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +from dataclasses import dataclass + +from isaacteleop.synchronization import ( + ClockStatus, + DeviceClockEstimator, + make_observation, +) + +NS_PER_SECOND = 1_000_000_000 + + +@dataclass(frozen=True) +class SimulatedClock: + epoch_host_ns: int + offset_ns: int + skew_ppm: float + + def device_ns(self, host_ns: int) -> int: + rate = 1.0 + self.skew_ppm * 1e-6 + return self.offset_ns + round((host_ns - self.epoch_host_ns) * rate) + + +def probe(clock: SimulatedClock, host_send_ns: int): + device_receive_host_ns = host_send_ns + 80_000 + device_send_host_ns = device_receive_host_ns + 20_000 + host_receive_ns = device_send_host_ns + 120_000 + return make_observation( + host_send_ns, + clock.device_ns(device_receive_host_ns), + clock.device_ns(device_send_host_ns), + host_receive_ns, + ) + + +def add_probes( + estimator: DeviceClockEstimator, clock: SimulatedClock, start_ns: int +) -> None: + for index in range(12): + estimator.update(probe(clock, start_ns + index * NS_PER_SECOND)) + + +def conversion_error_us( + estimator: DeviceClockEstimator, clock: SimulatedClock, host_ns: int +) -> float: + converted = estimator.to_local_common_ns(clock.device_ns(host_ns)) + if converted.status != ClockStatus.Synchronized: + raise RuntimeError(f"expected synchronized clock, got {converted.status}") + return (converted.local_common_ns - host_ns) / 1_000.0 + + +def main() -> None: + start_ns = 1_000 * NS_PER_SECOND + clock = SimulatedClock(start_ns, offset_ns=5 * NS_PER_SECOND, skew_ppm=20.0) + estimator = DeviceClockEstimator( + window_s=30.0, + min_span_s=5.0, + max_extrapolation_windows=2.0, + ) + + add_probes(estimator, clock, start_ns) + error_us = conversion_error_us(estimator, clock, start_ns + 11 * NS_PER_SECOND) + stats = estimator.stats() + print("calibrated clock") + print(f" estimated skew: {stats.skew_ppm:+.2f} ppm") + print(f" median RTT: {stats.rtt_ns / 1_000:.1f} us") + print(f" conversion error: {error_us:+.1f} us") + + stale_host_ns = start_ns + 72 * NS_PER_SECOND + stale = estimator.to_local_common_ns(clock.device_ns(stale_host_ns)) + print(f"after probe silence: {stale.status}") + + restarted = SimulatedClock(stale_host_ns, offset_ns=100_000_000, skew_ppm=-15.0) + estimator.update(probe(restarted, stale_host_ns)) + restarting = estimator.to_local_common_ns(restarted.device_ns(stale_host_ns)) + print(f"after restart: {restarting.status}") + + add_probes(estimator, restarted, stale_host_ns + NS_PER_SECOND) + error_us = conversion_error_us( + estimator, + restarted, + stale_host_ns + 12 * NS_PER_SECOND, + ) + stats = estimator.stats() + print("recalibrated clock") + print(f" resets: {stats.resets}") + print(f" estimated skew: {stats.skew_ppm:+.2f} ppm") + print(f" conversion error: {error_us:+.1f} us") + + +if __name__ == "__main__": + main()