From e4f235001bb5113a7fb39630df58c52a911f2ace Mon Sep 17 00:00:00 2001
From: Damien SIX <damien.six@ls2n.fr>
Date: Wed, 12 May 2021 12:16:18 +0200
Subject: [PATCH] fix timesync for timestamp sample (including SFINAE
 detection)

---
 msg/templates/urtps/RtpsTopics.cpp.em | 3 +++
 msg/templates/urtps/RtpsTopics.h.em   | 9 ++++++---
 2 files changed, 9 insertions(+), 3 deletions(-)

diff --git a/msg/templates/urtps/RtpsTopics.cpp.em b/msg/templates/urtps/RtpsTopics.cpp.em
index e9e1b27105..32586121ff 100644
--- a/msg/templates/urtps/RtpsTopics.cpp.em
+++ b/msg/templates/urtps/RtpsTopics.cpp.em
@@ -109,8 +109,11 @@ void RtpsTopics::publish(uint8_t topic_ID, char data_buffer[], size_t len)
 @[    end if]@
             // apply timestamp offset
             uint64_t timestamp = getMsgTimestamp(&st);
+            uint64_t timestamp_sample = getMsgTimestampSample(&st);
             _timesync->subtractOffset(timestamp);
             setMsgTimestamp(&st, timestamp);
+            _timesync->subtractOffset(timestamp_sample);
+            setMsgTimestampSample(&st, timestamp_sample);
             _@(topic)_pub.publish(&st);
 @[    if topic == 'Timesync' or topic == 'timesync']@
             }
diff --git a/msg/templates/urtps/RtpsTopics.h.em b/msg/templates/urtps/RtpsTopics.h.em
index 4f3f9d4e15..37d87c4fd2 100644
--- a/msg/templates/urtps/RtpsTopics.h.em
+++ b/msg/templates/urtps/RtpsTopics.h.em
@@ -120,10 +120,13 @@ private:
     // SFINAE
     template<typename T> struct hasTimestampSample{
     private:
-      static void detect(...);
-      template<typename U> static decltype(std::declval<U>().timestamp_sample()) detect(const U&);
+        template<typename U,
+                typename = decltype(std::declval<U>().timestamp_sample(int64_t()))>
+        static std::true_type detect(int);
+        template<typename U>
+        static std::false_type detect(...);
     public:
-      static constexpr bool value = std::is_same<uint64_t, decltype(detect(std::declval<T>()))>::value;
+        static constexpr bool value = decltype(detect<T>(0))::value;
     };
 
     template<typename T>