microRTPS: timesync: add getters and setter to generalize interface

msg/templates/urtps/RtpsTopics.cpp.em

@@ -110,6 +110,7 @@ void RtpsTopics::publish(uint8_t topic_ID, char data_buffer[], size_t len)
             // apply timestamp offset
             uint64_t timestamp = getMsgTimestamp(&st);
             _timesync->subtractOffset(timestamp);
+            setMsgTimestamp(&st, timestamp);
             _@(topic)_pub.publish(&st);
 @[    if topic == 'Timesync' or topic == 'timesync']@
             }
@@ -141,6 +142,7 @@ bool RtpsTopics::getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr)
                 // apply timestamp offset
                 uint64_t timestamp = getMsgTimestamp(&msg);
                 _timesync->addOffset(timestamp);
+                setMsgTimestamp(&msg, timestamp);
                 msg.serialize(scdr);
                 ret = true;
 @[    if topic == 'Timesync' or topic == 'timesync']@

msg/templates/urtps/RtpsTopics.h.em

@@ -118,32 +118,65 @@ public:
 
 private:
 @[if send_topics]@
-    // Publishers
+    /** Publishers **/
 @[for topic in send_topics]@
     @(topic)_Publisher _@(topic)_pub;
 @[end for]@
 @[end if]@
 
 @[if recv_topics]@
-    // Subscribers
+    /** Subscribers **/
 @[for topic in recv_topics]@
     @(topic)_Subscriber _@(topic)_sub;
 @[end for]@
 @[end if]@
 
+    /** Msg metada Getters **/
 @[if fastrtps_version <= 1.7 or not ros2_distro]@
     template <class T>
-    uint8_t getMsgSysID(T* msg) { return msg->sys_id_(); }
+    inline uint64_t getMsgTimestamp(const T* msg) { return msg->timestamp_(); }
 
     template <class T>
-    uint64_t getMsgTimestamp(T* msg) { return msg->timestamp_(); }
+    inline uint8_t getMsgSysID(const T* msg) { return msg->sys_id_(); }
+
+    template <class T>
+    inline uint8_t getMsgSeq(const T* msg) { return msg->seq_(); }
+@[elif ros2_distro]@
+    template <class T>
+    inline uint64_t getMsgTimestamp(const T* msg) { return msg->timestamp(); }
+
+    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]@
+
+    /** Msg metadata Getters **/
+@[if fastrtps_version <= 1.7 or not ros2_distro]@
+    template <class T>
+    inline uint64_t setMsgTimestamp(T* msg, const uint64_t& timestamp) { msg->timestamp_() = timestamp; }
+
+    template <class T>
+    inline uint8_t setMsgSysID(T* msg, const uint8_t& sys_id) { msg->sys_id_() = sys_id; }
+
+    template <class T>
+    inline uint8_t setMsgSeq(T* msg, const uint8_t& seq) { msg->seq_() = seq; }
 @[elif ros2_distro]@
     template <class T>
-    uint8_t getMsgSysID(T* msg) { return msg->sys_id(); }
+    inline uint64_t setMsgTimestamp(T* msg, const uint64_t& timestamp) { msg->timestamp() = timestamp; }
+
+    template <class T>
+    inline uint8_t setMsgSysID(T* msg, const uint8_t& sys_id) { msg->sys_id() = sys_id; }
 
     template <class T>
-    uint64_t getMsgTimestamp(T* msg) { return msg->timestamp(); }
+    inline uint8_t setMsgSeq(T* msg, const uint8_t& seq) { msg->seq() = seq; }
 @[end if]@
 
+    /**
+     * @@brief Timesync object ptr.
+     *         This object is used to compuyte and apply the time offsets to the
+     *         messages timestamps.
+     */
     std::shared_ptr<TimeSync> _timesync;
 };

msg/templates/urtps/microRTPS_timesync.cpp.em

@@ -163,19 +163,19 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t
 }
 
 void TimeSync::processTimesyncMsg(timesync_msg_t * msg) {
-	if (msg->sys_id() == 1 && msg->seq() != _last_remote_msg_seq) {
-                _last_remote_msg_seq = msg->seq();
+	if (getMsgSysID(msg) == 1 && getMsgSeq(msg) != _last_remote_msg_seq) {
+                _last_remote_msg_seq = getMsgSeq(msg);
 
-		if (msg->tc1() > 0) {
-			if (!addMeasurement(msg->ts1(), msg->tc1(), getMonoRawTimeNSec())) {
+		if (getMsgTC1(msg) > 0) {
+			if (!addMeasurement(getMsgTS1(msg), getMsgTC1(msg), getMonoRawTimeNSec())) {
 				std::cerr << "Offset not updated" << std::endl;
 			}
 
-		} else if (msg->tc1() == 0) {
-			msg->timestamp() = getMonoTimeUSec();
-			msg->sys_id() = 0;
-			msg->seq()++;
-			msg->tc1() = getMonoRawTimeNSec();
+		} else if (getMsgTC1(msg) == 0) {
+			setMsgTimestamp(msg, getMonoTimeUSec());
+			setMsgSysID(msg, 0);
+			setMsgSeq(msg, getMsgSeq(msg) + 1);
+			setMsgTC1(msg, getMonoRawTimeNSec());
 
 			_timesync_pub.publish(msg);
 		}
@@ -185,11 +185,11 @@ void TimeSync::processTimesyncMsg(timesync_msg_t * msg) {
 timesync_msg_t TimeSync::newTimesyncMsg() {
 	timesync_msg_t msg{};
 
-	msg.timestamp() = getMonoTimeUSec();
-	msg.sys_id() = 0;
-	msg.seq() = _last_msg_seq;
-	msg.tc1() = 0;
-	msg.ts1() = getMonoRawTimeNSec();
+	setMsgTimestamp(&msg, getMonoTimeUSec());
+	setMsgSysID(&msg, 0);
+	setMsgSeq(&msg, _last_msg_seq);
+	setMsgTC1(&msg, 0);
+	setMsgTS1(&msg, getMonoRawTimeNSec());
 
 	_last_msg_seq++;
 

msg/templates/urtps/microRTPS_timesync.h.em

@@ -211,4 +211,34 @@ private:
 	 * @@param[in] offset The value of the offset to update to
 	 */
 	inline void updateOffset(const uint64_t& offset) { _offset_ns.store(offset, std::memory_order_relaxed); }
+
+        /** Timesync msg Getters **/
+@[if fastrtps_version <= 1.7 or not ros2_distro]@
+	inline uint64_t getMsgTimestamp(const timesync_msg_t* msg) { return msg->timestamp_(); }
+	inline uint8_t getMsgSysID(const timesync_msg_t* msg) { return msg->sys_id_(); }
+	inline uint8_t getMsgSeq(const timesync_msg_t* msg) { return msg->seq_(); }
+	inline int64_t getMsgTC1(const timesync_msg_t* msg) { return msg->tc1_(); }
+	inline int64_t getMsgTS1(const timesync_msg_t* msg) { return msg->ts1_(); }
+@[elif ros2_distro]@
+	inline uint64_t getMsgTimestamp(const timesync_msg_t* msg) { return msg->timestamp(); }
+	inline uint8_t getMsgSysID(const timesync_msg_t* msg) { return msg->sys_id(); }
+	inline uint8_t getMsgSeq(const timesync_msg_t* msg) { return msg->seq(); }
+	inline int64_t getMsgTC1(const timesync_msg_t* msg) { return msg->tc1(); }
+	inline int64_t getMsgTS1(const timesync_msg_t* msg) { return msg->ts1(); }
+@[end if]@
+
+        /** Timesync msg Setters **/
+@[if fastrtps_version <= 1.7 or not ros2_distro]@
+	inline uint64_t setMsgTimestamp(timesync_msg_t* msg, const uint64_t& timestamp) { msg->timestamp_() = timestamp; }
+	inline uint8_t setMsgSysID(timesync_msg_t* msg, const uint8_t& sys_id) { msg->sys_id_() = sys_id; }
+	inline uint8_t setMsgSeq(timesync_msg_t* msg, const uint8_t& seq) { msg->seq_() = seq; }
+	inline int64_t setMsgTC1(timesync_msg_t* msg, const int64_t& tc1) { msg->tc1_() = tc1; }
+	inline int64_t setMsgTS1(timesync_msg_t* msg, const int64_t& ts1) { msg->ts1_() = ts1; }
+@[elif ros2_distro]@
+	inline uint64_t setMsgTimestamp(timesync_msg_t* msg, const uint64_t& timestamp) { msg->timestamp() = timestamp; }
+	inline uint8_t setMsgSysID(timesync_msg_t* msg, const uint8_t& sys_id) { msg->sys_id() = sys_id; }
+	inline uint8_t setMsgSeq(timesync_msg_t* msg, const uint8_t& seq) { msg->seq() = seq; }
+	inline int64_t setMsgTC1(timesync_msg_t* msg, const int64_t& tc1) { msg->tc1() = tc1; }
+	inline int64_t setMsgTS1(timesync_msg_t* msg, const int64_t& ts1) { msg->ts1() = ts1; }
+@[end if]@
 };