diff --git a/msg/templates/urtps/RtpsTopics.cpp.em b/msg/templates/urtps/RtpsTopics.cpp.em index 76b6e411774..367498862dd 100644 --- a/msg/templates/urtps/RtpsTopics.cpp.em +++ b/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']@ diff --git a/msg/templates/urtps/RtpsTopics.h.em b/msg/templates/urtps/RtpsTopics.h.em index 0a5c0dd0c87..809dc5e9ab7 100644 --- a/msg/templates/urtps/RtpsTopics.h.em +++ b/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 - uint8_t getMsgSysID(T* msg) { return msg->sys_id_(); } + inline uint64_t getMsgTimestamp(const T* msg) { return msg->timestamp_(); } template - uint64_t getMsgTimestamp(T* msg) { return msg->timestamp_(); } + inline uint8_t getMsgSysID(const T* msg) { return msg->sys_id_(); } + + template + inline uint8_t getMsgSeq(const T* msg) { return msg->seq_(); } @[elif ros2_distro]@ template - uint8_t getMsgSysID(T* msg) { return msg->sys_id(); } + inline uint64_t getMsgTimestamp(const T* msg) { return msg->timestamp(); } template - uint64_t getMsgTimestamp(T* msg) { return msg->timestamp(); } + inline uint8_t getMsgSysID(const T* msg) { return msg->sys_id(); } + + template + 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 + inline uint64_t setMsgTimestamp(T* msg, const uint64_t& timestamp) { msg->timestamp_() = timestamp; } + + template + inline uint8_t setMsgSysID(T* msg, const uint8_t& sys_id) { msg->sys_id_() = sys_id; } + + template + inline uint8_t setMsgSeq(T* msg, const uint8_t& seq) { msg->seq_() = seq; } +@[elif ros2_distro]@ + template + inline uint64_t setMsgTimestamp(T* msg, const uint64_t& timestamp) { msg->timestamp() = timestamp; } + + template + inline uint8_t setMsgSysID(T* msg, const uint8_t& sys_id) { msg->sys_id() = sys_id; } + + template + 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; }; diff --git a/msg/templates/urtps/microRTPS_timesync.cpp.em b/msg/templates/urtps/microRTPS_timesync.cpp.em index 09cbfa70a8c..612add5b499 100644 --- a/msg/templates/urtps/microRTPS_timesync.cpp.em +++ b/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++; diff --git a/msg/templates/urtps/microRTPS_timesync.h.em b/msg/templates/urtps/microRTPS_timesync.h.em index ef137b236e2..bfd6a8a6a61 100644 --- a/msg/templates/urtps/microRTPS_timesync.h.em +++ b/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]@ };