RTPS: adjust timestamp_sample in urtps_agent

PX4 · Feb 17, 2021 · 51dd141 · 51dd141
1 parent e48a869
commit 51dd141
Show file tree

Hide file tree

Showing 2 changed files with 44 additions and 1 deletion.
diff --git a/msg/templates/urtps/RtpsTopics.cpp.em b/msg/templates/urtps/RtpsTopics.cpp.em
@@ -139,10 +139,13 @@ bool RtpsTopics::getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr)
 @[    if topic == 'Timesync' or topic == 'timesync']@
                 if (getMsgSysID(&msg) == 0) {
 @[    end if]@
-                // apply timestamp offset
+                // apply timestamps offset
                 uint64_t timestamp = getMsgTimestamp(&msg);
+                uint64_t timestamp_sample = getMsgTimestampSample(&msg);
                 _timesync->addOffset(timestamp);
                 setMsgTimestamp(&msg, timestamp);
+                _timesync->addOffset(timestamp_sample);
+                setMsgTimestampSample(&msg, timestamp_sample);
                 msg.serialize(scdr);
                 ret = true;
 @[    if topic == 'Timesync' or topic == 'timesync']@

diff --git a/msg/templates/urtps/RtpsTopics.h.em b/msg/templates/urtps/RtpsTopics.h.em
@@ -63,6 +63,7 @@ except AttributeError:
 #include <fastcdr/Cdr.h>
 #include <condition_variable>
 #include <queue>
+#include <type_traits>
 
 #include "microRTPS_timesync.h"
 
@@ -116,11 +117,28 @@ private:
 @[end for]@
 @[end if]@
 
+    // SFINAE
+    template<typename T> struct hasTimestampSample{
+    private:
+      static void detect(...);
+      template<typename U> static decltype(std::declval<U>().timestamp_sample()) detect(const U&);
+    public:
+      static constexpr bool value = std::is_same<uint64_t, decltype(detect(std::declval<T>()))>::value;
+    };
+
+    template<typename T>
+    inline typename std::enable_if<!hasTimestampSample<T>::value, uint64_t>::type
+    getMsgTimestampSample_impl(const T*) { return 0; }
+
     /** Msg metada Getters **/
 @[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@
     template <class T>
     inline uint64_t getMsgTimestamp(const T* msg) { return msg->timestamp_(); }
 
+    template<typename T>
+    inline typename std::enable_if<hasTimestampSample<T>::value, uint64_t>::type
+    getMsgTimestampSample_impl(const T* msg) { return msg->timestamp_sample_(); }
+
     template <class T>
     inline uint8_t getMsgSysID(const T* msg) { return msg->sys_id_(); }
 
@@ -130,18 +148,33 @@ private:
     template <class T>
     inline uint64_t getMsgTimestamp(const T* msg) { return msg->timestamp(); }
 
+    template<typename T>
+    inline typename std::enable_if<hasTimestampSample<T>::value, uint64_t>::type
+    getMsgTimestampSample_impl(const T* msg) { return msg->timestamp_sample(); }
+
     template <class T>
     inline uint8_t getMsgSysID(const T* msg) { return msg->sys_id(); }
 
     template <class T>
     inline uint8_t getMsgSeq(const T* msg) { return msg->seq(); }
 @[end if]@
 
+    template <class T>
+    inline uint64_t getMsgTimestampSample(const T* msg) { return getMsgTimestampSample_impl(msg); }
+
+    template<typename T>
+    inline typename std::enable_if<!hasTimestampSample<T>::value, void>::type
+    setMsgTimestampSample_impl(T*, const uint64_t&) {}
+
     /** Msg metadata Setters **/
 @[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@
     template <class T>
     inline void setMsgTimestamp(T* msg, const uint64_t& timestamp) { msg->timestamp_() = timestamp; }
 
+    template <class T>
+    inline typename std::enable_if<hasTimestampSample<T>::value, void>::type
+    setMsgTimestampSample_impl(T* msg, const uint64_t& timestamp_sample) { msg->timestamp_sample_() = timestamp_sample; }
+
     template <class T>
     inline void setMsgSysID(T* msg, const uint8_t& sys_id) { msg->sys_id_() = sys_id; }
 
@@ -151,13 +184,20 @@ private:
     template <class T>
     inline void setMsgTimestamp(T* msg, const uint64_t& timestamp) { msg->timestamp() = timestamp; }
 
+    template <class T>
+    inline typename std::enable_if<hasTimestampSample<T>::value, void>::type
+    setMsgTimestampSample_impl(T* msg, const uint64_t& timestamp_sample) { msg->timestamp_sample() = timestamp_sample; }
+
     template <class T>
     inline void setMsgSysID(T* msg, const uint8_t& sys_id) { msg->sys_id() = sys_id; }
 
     template <class T>
     inline void setMsgSeq(T* msg, const uint8_t& seq) { msg->seq() = seq; }
 @[end if]@
 
+    template <class T>
+    inline void setMsgTimestampSample(T* msg, const uint64_t& timestamp_sample) { setMsgTimestampSample_impl(msg, timestamp_sample); }
+
     /**
      * @@brief Timesync object ptr.
      *         This object is used to compuyte and apply the time offsets to the