microRTPS: timesync: cleanup and style fix

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<int64_t>((_skew_ns_per_sync + _offset_ns.load()) * (1. - alpha) + measurement_offset * alpha));
-	_skew_ns_per_sync = static_cast<int64_t>(beta * (_offset_ns.load() - offset_prev) + (1. - beta) * _skew_ns_per_sync);
+	updateOffset(static_cast<int64_t>((_skew_ns_per_sync + _offset_ns.load()) * (1. - alpha) +
+					  measurement_offset * alpha));
+	_skew_ns_per_sync =
+	    static_cast<int64_t>(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;

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<int64_t>(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<int64_t>(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);
-
-	@(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);
+	/**
+	 * @@brief Processes DDS timesync message
+	 * @@param[in,out] msg The timestamp msg to be processed
+	 */
+	void processTimesyncMsg(timesync_msg_t* 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(); }