Normalize rmw_time_t according to DDS spec

rmw_dds_common/src/time_utils.cpp

@@ -24,21 +24,37 @@ rmw_dds_common::clamp_rmw_time_to_dds_time(const rmw_time_t & time)
 {
   rmw_time_t t = time;
 
-  // Let's try to keep the seconds value representable with 32-bits
-  // by moving the excess seconds over to the nanoseconds field.
-  if (t.sec > INT_MAX) {
-    uint64_t diff = t.sec - INT_MAX;
-    t.sec -= diff;
-    t.nsec += diff * (1000LL * 1000LL * 1000LL);
+  // Normalize rmw_time_t::nsec to be < 1s, so that it may be safely converted
+  // to a DDS Duration or Time (see DDS v1.4 section 2.3.2).
+  // If the total length in seconds cannot be represented by DDS (which is
+  // limited to INT_MAX seconds + (10^9 - 1) nanoseconds) we must truncate
+  // the seconds component at INT_MAX, while also normalizing nanosec to < 1s.
+  constexpr uint64_t sec_to_ns = 1000000000ULL;
+  uint64_t ns_sec_adjust = t.nsec / sec_to_ns;
+  bool overflow_nsec = false;
+  bool overflow_sec = false;
+
+  if (ns_sec_adjust > INT_MAX) {
+    ns_sec_adjust = INT_MAX;
+    overflow_nsec = true;
+  }
+
+  if (t.sec > INT_MAX - ns_sec_adjust) {
+    t.sec = INT_MAX;
+    overflow_sec = true;
+  } else {
+    t.sec += ns_sec_adjust;
   }
 
-  // In case the nanoseconds value is too large for a 32-bit unsigned,
-  // we can at least saturate and emit a warning
-  if (t.nsec > UINT_MAX) {
-    RCUTILS_LOG_WARN_NAMED(
+  if (overflow_nsec || overflow_sec) {
+    // The nsec component must be "saturated" if we are overflowing INT_MAX
+    t.nsec = sec_to_ns - 1;
+    RCUTILS_LOG_DEBUG_NAMED(
       "rmw_dds_common",
-      "nanoseconds value too large for 32-bits, saturated at UINT_MAX");
-    t.nsec = UINT_MAX;
+      "rmw_time_t length cannot be represented by DDS, truncated at "
+      "INT_MAX seconds + (10^9 - 1) nanoseconds");
+  } else {
+    t.nsec = t.nsec - ns_sec_adjust * sec_to_ns;
   }
 
   return t;

rmw_dds_common/test/test_time_utils.cpp

@@ -32,23 +32,65 @@ TEST(test_time_utils, test_unmodified_zeros)
 
 TEST(test_time_utils, test_unmodified_max)
 {
-  const rmw_time_t max_dds_time {0x7FFFFFFF, 0xFFFFFFFF};
+  // Maximum time length supported by DDS is { INT_MAX, 10^9 - 1 }
+  const rmw_time_t max_dds_time {0x7FFFFFFF, 999999999ULL};
   auto t2 = rmw_dds_common::clamp_rmw_time_to_dds_time(max_dds_time);
   EXPECT_TRUE(t2 == max_dds_time);
 }
 
 TEST(test_time_utils, test_seconds_overflow)
 {
-  const rmw_time_t slightly_too_large {2147483651, 0};
-  auto t3 = rmw_dds_common::clamp_rmw_time_to_dds_time(slightly_too_large);
-  const rmw_time_t result3 {0x7FFFFFFF, 4000000000};
-  EXPECT_TRUE(t3 == result3);
+  // Maximum time length supported by DDS is { INT_MAX, 10^9 - 1 }
+  const rmw_time_t slightly_too_large {0x80000000, 0};
+  auto t3_sec = rmw_dds_common::clamp_rmw_time_to_dds_time(slightly_too_large);
+  const rmw_time_t result3 {0x7FFFFFFF, 999999999ULL};
+  EXPECT_TRUE(t3_sec == result3);
+  // Set the whole length via the nanosec field.
+  const rmw_time_t slightly_too_large_ns {0, 0x80000000 * 1000000000ULL};
+  auto t3_ns = rmw_dds_common::clamp_rmw_time_to_dds_time(slightly_too_large_ns);
+  EXPECT_TRUE(t3_ns == result3);
+
+  const rmw_time_t slightly_too_large_both_1 {0x7FFFFFFF, 1000000000ULL};
+  auto t3_both_1 = rmw_dds_common::clamp_rmw_time_to_dds_time(slightly_too_large_both_1);
+  EXPECT_TRUE(t3_both_1 == result3);
+
+  const rmw_time_t slightly_too_large_both_2 {0x80000000, 9999999998ULL};
+  auto t3_both_2 = rmw_dds_common::clamp_rmw_time_to_dds_time(slightly_too_large_both_2);
+  EXPECT_TRUE(t3_both_2 == result3);
 }
 
 TEST(test_time_utils, test_saturation)
 {
   const rmw_time_t max_64 {LLONG_MAX, ULLONG_MAX};
-  const rmw_time_t max_dds_time {0x7FFFFFFF, 0xFFFFFFFF};
+  // Maximum time length supported by DDS is { INT_MAX, 10^9 - 1 }
+  const rmw_time_t max_dds_time {0x7FFFFFFF, 999999999ULL};
   auto t4 = rmw_dds_common::clamp_rmw_time_to_dds_time(max_64);
   EXPECT_TRUE(t4 == max_dds_time);
 }
+
+TEST(test_time_utils, test_normalize)
+{
+  // The DDS Spec requires that the nanoseconds fields be < 1s.
+  const rmw_time_t already_normalized {1, 999999999ULL};
+  auto already_normalized_res = rmw_dds_common::clamp_rmw_time_to_dds_time(already_normalized);
+  EXPECT_TRUE(already_normalized_res == already_normalized);
+
+  const rmw_time_t unnormalized_min {0, 1000000000ULL};
+  auto normalized_min = rmw_dds_common::clamp_rmw_time_to_dds_time(unnormalized_min);
+  const rmw_time_t normalized_min_expected {1, 0};
+  EXPECT_TRUE(normalized_min == normalized_min_expected);
+
+  const rmw_time_t unnormalized_mid {0, 0x5FFFFFFF * 1000000000ULL + 999999999ULL};
+  auto normalized_mid = rmw_dds_common::clamp_rmw_time_to_dds_time(unnormalized_mid);
+  const rmw_time_t normalized_mid_expected {0x5FFFFFFF, 999999999ULL};
+  EXPECT_TRUE(normalized_mid == normalized_mid_expected);
+
+  const rmw_time_t unnormalized_max {0, 0x7FFFFFFF * 1000000000ULL + 999999999ULL};
+  auto normalized_max = rmw_dds_common::clamp_rmw_time_to_dds_time(unnormalized_max);
+  const rmw_time_t normalized_max_expected {0x7FFFFFFF, 999999999ULL};
+  EXPECT_TRUE(normalized_max == normalized_max_expected);
+
+  const rmw_time_t unnormalized_max_2 {0x7FFFFFFE, 1999999999ULL};
+  auto normalized_max_2 = rmw_dds_common::clamp_rmw_time_to_dds_time(unnormalized_max_2);
+  EXPECT_TRUE(normalized_max_2 == normalized_max_expected);
+}