From 10113820985805e172607083dfff11bbc794ebab Mon Sep 17 00:00:00 2001 From: Julian Kent Date: Tue, 16 Mar 2021 16:16:10 +0100 Subject: [PATCH] Clamp RTPS -> uorb timestamps to system time --- msg/templates/uorb_microcdr/microRTPS_client.cpp.em | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/msg/templates/uorb_microcdr/microRTPS_client.cpp.em b/msg/templates/uorb_microcdr/microRTPS_client.cpp.em index d6cff18527..6953f5e9e5 100644 --- a/msg/templates/uorb_microcdr/microRTPS_client.cpp.em +++ b/msg/templates/uorb_microcdr/microRTPS_client.cpp.em @@ -218,6 +218,7 @@ void micrortps_start_topics(struct timespec &begin, uint64_t &total_read, uint64 while (0 < (read = transport_node->read(&topic_ID, data_buffer, BUFFER_SIZE))) { total_read += read; + uint64_t read_time = hrt_absolute_time(); switch (topic_ID) { @[for idx, topic in enumerate(recv_topics)]@ @@ -225,6 +226,11 @@ void micrortps_start_topics(struct timespec &begin, uint64_t &total_read, uint64 { @(receive_base_types[idx])_s @(topic)_data; deserialize_@(receive_base_types[idx])(&reader, &@(topic)_data, data_buffer); + if (@(topic)_data.timestamp > read_time) + { + // don't allow timestamps from the future + @(topic)_data.timestamp = read_time; + } pubs->@(topic)_pub.publish(@(topic)_data); ++received; }