From 04ec2041e3915a522a3d2f3ddc2d0fde6ceae70a Mon Sep 17 00:00:00 2001 From: TSC21 Date: Tue, 10 Mar 2020 11:32:16 +0000 Subject: [PATCH] microRTPS: timesync: cleanup and style fix --- msg/templates/urtps/microRTPS_timesync.cpp.em | 93 +++++-------------- msg/templates/urtps/microRTPS_timesync.h.em | 87 +++++++++++------ 2 files changed, 82 insertions(+), 98 deletions(-) diff --git a/msg/templates/urtps/microRTPS_timesync.cpp.em b/msg/templates/urtps/microRTPS_timesync.cpp.em index ade3fd2800..6ef8e2f3c6 100644 --- a/msg/templates/urtps/microRTPS_timesync.cpp.em +++ b/msg/templates/urtps/microRTPS_timesync.cpp.em @@ -73,34 +73,18 @@ TimeSync::TimeSync() _request_reset_counter(0), _last_msg_seq(0), _last_remote_msg_seq(0) -{} +{ } TimeSync::~TimeSync() { stop(); } -@[if ros2_distro]@ -void TimeSync::start(const Timesync_Publisher* pub) { -@[else]@ -void TimeSync::start(const timesync_Publisher* pub) { -@[end if]@ +void TimeSync::start(const TimesyncPublisher* pub) { stop(); _timesync_pub = (*pub); auto run = [this]() { while (!_request_stop) { -@[if 1.5 <= fastrtpsgen_version <= 1.7]@ -@[ if ros2_distro]@ - @(package)::msg::dds_::Timesync_ msg = newTimesyncMsg(); -@[ else]@ - timesync_ msg = newTimesyncMsg(); -@[ end if]@ -@[else]@ -@[ if ros2_distro]@ - @(package)::msg::Timesync msg = newTimesyncMsg(); -@[ else]@ - timesync msg = newTimesyncMsg(); -@[ end if]@ -@[end if]@ + timesync_msg_t msg = newTimesyncMsg(); _timesync_pub.publish(&msg); @@ -135,6 +119,11 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t std::cout << std::endl << "Timesync clock changed, resetting" << std::endl; } + if (_num_samples == 0) { + updateOffset(measurement_offset); + _skew_ns_per_sync = 0; + } + if (_num_samples >= WINDOW_SIZE) { if (std::abs(measurement_offset - _offset_ns.load()) > TRIGGER_RESET_THRESHOLD_NS) { _request_reset_counter++; @@ -145,11 +134,6 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t } } - if (_num_samples == 0) { - updateOffset(measurement_offset); - _skew_ns_per_sync = 0; - } - // ignore if rtti > 10ms if (rtti > 15ll * 1000ll * 1000ll) { std::cout << std::endl @@ -157,49 +141,37 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t return false; } - double alpha = ALPHA_FINAL; - double beta = BETA_FINAL; + double alpha = ALPHA_FINAL; + double beta = BETA_FINAL; - if (_num_samples < WINDOW_SIZE) { - double schedule = (double)_num_samples / WINDOW_SIZE; - double s = 1. - exp(.5 * (1. - 1. / (1. - schedule))); - alpha = (1. - s) * ALPHA_INITIAL + s * ALPHA_FINAL; - beta = (1. - s) * BETA_INITIAL + s * BETA_FINAL; - } + if (_num_samples < WINDOW_SIZE) { + double schedule = (double)_num_samples / WINDOW_SIZE; + double s = 1. - exp(.5 * (1. - 1. / (1. - schedule))); + alpha = (1. - s) * ALPHA_INITIAL + s * ALPHA_FINAL; + beta = (1. - s) * BETA_INITIAL + s * BETA_FINAL; + } int64_t offset_prev = _offset_ns.load(); - updateOffset(static_cast((_skew_ns_per_sync + _offset_ns.load()) * (1. - alpha) + measurement_offset * alpha)); - _skew_ns_per_sync = static_cast(beta * (_offset_ns.load() - offset_prev) + (1. - beta) * _skew_ns_per_sync); + updateOffset(static_cast((_skew_ns_per_sync + _offset_ns.load()) * (1. - alpha) + + measurement_offset * alpha)); + _skew_ns_per_sync = + static_cast(beta * (_offset_ns.load() - offset_prev) + (1. - beta) * _skew_ns_per_sync); _num_samples++; return true; } -@[if 1.5 <= fastrtpsgen_version <= 1.7]@ -@[ if ros2_distro]@ -void TimeSync::processTimesyncMsg(@(package)::msg::dds_::Timesync_* msg) { -@[ else]@ -void TimeSync::processTimesyncMsg(timesync_* msg) { -@[ end if]@ -@[else]@ -@[ if ros2_distro]@ -void TimeSync::processTimesyncMsg(@(package)::msg::Timesync* msg) { -@[ else]@ -void TimeSync::processTimesyncMsg(timesync* msg) { -@[ end if]@ -@[end if]@ +void TimeSync::processTimesyncMsg(timesync_msg_t * msg) { if (msg->sys_id() == 1 && msg->seq() != _last_remote_msg_seq) { - if (msg->tc1() > 0) { - _last_remote_msg_seq = msg->seq(); + _last_remote_msg_seq = msg->seq(); + if (msg->tc1() > 0) { if (!addMeasurement(msg->ts1(), msg->tc1(), getMonoRawTimeNSec())) { std::cerr << "Offset not updated" << std::endl; } } else if (msg->tc1() == 0) { - _last_remote_msg_seq = msg->seq(); - msg->timestamp() = getMonoTimeUSec(); msg->sys_id() = 0; msg->seq()++; @@ -210,23 +182,8 @@ void TimeSync::processTimesyncMsg(timesync* msg) { } } -@[if 1.5 <= fastrtpsgen_version <= 1.7]@ -@[ if ros2_distro]@ -@(package)::msg::dds_::Timesync_ TimeSync::newTimesyncMsg() { - @(package)::msg::dds_::Timesync_ msg{}; -@[ else]@ -timesync_ TimeSync::newTimesyncMsg() { - timesync_ msg{}; -@[ end if]@ -@[else]@ -@[ if ros2_distro]@ -@(package)::msg::Timesync TimeSync::newTimesyncMsg() { - @(package)::msg::Timesync msg{}; -@[ else]@ -timesync TimeSync::newTimesyncMsg() { - timesync msg{}; -@[ end if]@ -@[end if]@ +timesync_msg_t TimeSync::newTimesyncMsg() { + timesync_msg_t msg{}; msg.timestamp() = getMonoTimeUSec(); msg.sys_id() = 0; diff --git a/msg/templates/urtps/microRTPS_timesync.h.em b/msg/templates/urtps/microRTPS_timesync.h.em index 8f57d54cf8..c014a69184 100644 --- a/msg/templates/urtps/microRTPS_timesync.h.em +++ b/msg/templates/urtps/microRTPS_timesync.h.em @@ -85,65 +85,92 @@ static constexpr int64_t UNKNOWN = 0; static constexpr int64_t TRIGGER_RESET_THRESHOLD_NS = 100ll * 1000ll * 1000ll; static constexpr int REQUEST_RESET_COUNTER_THRESHOLD = 5; +@# Sets the timesync DDS type according to the FastRTPS and ROS2 version +@[if 1.5 <= fastrtpsgen_version <= 1.7]@ +@[ if ros2_distro]@ +using timesync_msg_t = @(package)::msg::dds_::Timesync_; +@[ else]@ +using timesync_msg_t = timesync_; +@[ end if]@ +@[else]@ +@[ if ros2_distro]@ +using timesync_msg_t = @(package)::msg::Timesync; +@[ else]@ +using timesync_msg_t = timesync; +@[ end if]@ +@[end if]@ +@# Sets the timesync publisher entity depending on using ROS2 or not +@[if ros2_distro]@ +using TimesyncPublisher = Timesync_Publisher; +@[else]@ +using TimesyncPublisher = timesync_Publisher; +@[end if]@ + class TimeSync { public: TimeSync(); virtual ~TimeSync(); -@[if ros2_distro]@ - void start(const Timesync_Publisher* pub); -@[else]@ - void start(const timesync_Publisher* pub); -@[end if]@ + /** + * @@brief Starts the timesync publishing thread + * @@param[in] pub The timesync publisher entity to use + */ + void start(const TimesyncPublisher* pub); + /** + * @@brief Resets the filter + */ void reset(); + + /** + * @@brief Stops the timesync publishing thread + */ void stop(); /** - * Get clock monotonic time (raw) in nanoseconds + * @@brief Get clock monotonic time (raw) in nanoseconds + * @@return System CLOCK_MONOTONIC_RAW time in nanoseconds */ inline int64_t getMonoRawTimeNSec() { timespec t; clock_gettime(CLOCK_MONOTONIC_RAW, &t); - return (int64_t)t.tv_sec * 1000000000LL + t.tv_nsec; + return static_cast(t.tv_sec * 1000000000LL + t.tv_nsec); } /** - * Get system monotonic time in microseconds + * @@brief Get system monotonic time in microseconds + * @@return System CLOCK_MONOTONIC time in microseconds */ inline int64_t getMonoTimeUSec() { timespec t; clock_gettime(CLOCK_MONOTONIC, &t); - return (int64_t)(t.tv_sec * 1000000000LL + t.tv_nsec) / 1000LL; + return static_cast(t.tv_sec * 1000000000LL + t.tv_nsec) / 1000LL; } + /** + * @@brief Adds a time offset measurement to be filtered + * @@param[in] local_t1_ns The agent CLOCK_MONOTONIC_RAW time in nanoseconds when the message was sent + * @@param[in] remote_t2_ns The (client) remote CLOCK_MONOTONIC time in nanoseconds + * @@param[in] local_t3_ns The agent current CLOCK_MONOTONIC time in nanoseconds + * @@return true or false depending if the time offset was updated + */ bool addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t local_t3_ns); -@[if 1.5 <= fastrtpsgen_version <= 1.7]@ -@[ if ros2_distro]@ - void processTimesyncMsg(@(package)::msg::dds_::Timesync_* msg); + /** + * @@brief Processes DDS timesync message + * @@param[in,out] msg The timestamp msg to be processed + */ + void processTimesyncMsg(timesync_msg_t* msg); - @(package)::msg::dds_::Timesync_ newTimesyncMsg(); -@[ else]@ - void processTimesyncMsg(timesync_* msg); - - timesync_ newTimesyncMsg(); -@[ end if]@ -@[else]@ -@[ if ros2_distro]@ - void processTimesyncMsg(@(package)::msg::Timesync* msg); - - @(package)::msg::Timesync newTimesyncMsg(); -@[ else]@ - void processTimesyncMsg(timesync* msg); - - timesync newTimesyncMsg(); -@[ end if]@ -@[end if]@ + /** + * @@brief Creates a new timesync DDS message to be sent from the agent to the client + * @@return A new timesync message with the origin in the agent and with the agent timestamp + */ + timesync_msg_t newTimesyncMsg(); /** * @@brief Get the time sync offset in nanoseconds - * @@return The offset in nanosecods + * @@return The offset in nanoseconds */ inline int64_t getOffset() { return _offset_ns.load(); }