From 98f53c2bc4d82295d01408c1402d48f4ff1ef170 Mon Sep 17 00:00:00 2001
From: TSC21 <n.marques21@hotmail.com>
Date: Thu, 27 Aug 2020 16:07:18 +0200
Subject: [PATCH] microRTPS: extend verbose arg to the timesync

---
 msg/templates/urtps/microRTPS_agent.cpp.em    |  8 ++++----
 msg/templates/urtps/microRTPS_timesync.cpp.em | 13 +++++++------
 msg/templates/urtps/microRTPS_timesync.h.em   |  4 +++-
 3 files changed, 14 insertions(+), 11 deletions(-)

diff --git a/msg/templates/urtps/microRTPS_agent.cpp.em b/msg/templates/urtps/microRTPS_agent.cpp.em
index 0435eab905..13a499cb20 100644
--- a/msg/templates/urtps/microRTPS_agent.cpp.em
+++ b/msg/templates/urtps/microRTPS_agent.cpp.em
@@ -90,9 +90,6 @@ Transport_node *transport_node = nullptr;
 RtpsTopics topics;
 uint32_t total_sent = 0, sent = 0;
 
-// Init timesync
-std::shared_ptr<TimeSync> timeSync = std::make_shared<TimeSync>();
-
 struct options {
     enum class eTransports
     {
@@ -174,7 +171,6 @@ void signal_handler(int signum)
    printf("\033[1;33m[   micrortps_agent   ]\tInterrupt signal (%d) received.\033[0m\n", signum);
    running = 0;
    transport_node->close();
-   timeSync->stop();
 }
 
 @[if recv_topics]@
@@ -271,6 +267,9 @@ int main(int argc, char** argv)
     std::chrono::time_point<std::chrono::steady_clock> start, end;
 @[end if]@
 
+    // Init timesync
+    std::shared_ptr<TimeSync> timeSync = std::make_shared<TimeSync>(_options.verbose_debug);
+
     topics.set_timesync(timeSync);
 
 @[if recv_topics]@
@@ -319,6 +318,7 @@ int main(int argc, char** argv)
     delete transport_node;
     transport_node = nullptr;
 
+    timeSync->stop();
     timeSync->reset();
 
     return 0;
diff --git a/msg/templates/urtps/microRTPS_timesync.cpp.em b/msg/templates/urtps/microRTPS_timesync.cpp.em
index 5588b181e1..51ada802e8 100644
--- a/msg/templates/urtps/microRTPS_timesync.cpp.em
+++ b/msg/templates/urtps/microRTPS_timesync.cpp.em
@@ -66,13 +66,14 @@ except AttributeError:
 
 #include "microRTPS_timesync.h"
 
-TimeSync::TimeSync()
+TimeSync::TimeSync(bool debug)
     : _offset_ns(-1),
       _skew_ns_per_sync(0.0),
       _num_samples(0),
       _request_reset_counter(0),
       _last_msg_seq(0),
-      _last_remote_msg_seq(0)
+      _last_remote_msg_seq(0),
+      _debug(debug)
 { }
 
 TimeSync::~TimeSync() { stop(); }
@@ -116,7 +117,7 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t
 
 	if (_request_reset_counter > REQUEST_RESET_COUNTER_THRESHOLD) {
 		reset();
-		std::cout << "\033[1;33m[ micrortps__timesync ]\tTimesync clock changed, resetting\033[0m" << std::endl;
+		if (_debug) std::cout << "\033[1;33m[ micrortps__timesync ]\tTimesync clock changed, resetting\033[0m" << std::endl;
 	}
 
         if (_num_samples == 0) {
@@ -127,7 +128,7 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t
 	if (_num_samples >= WINDOW_SIZE) {
 		if (std::abs(measurement_offset - _offset_ns.load()) > TRIGGER_RESET_THRESHOLD_NS) {
 			_request_reset_counter++;
-			std::cout << "\033[1;33m[ micrortps__timesync ]\tTimesync offset outlier, discarding\033[0m" << std::endl;
+			if (_debug) std::cout << "\033[1;33m[ micrortps__timesync ]\tTimesync offset outlier, discarding\033[0m" << std::endl;
 			return false;
 		} else {
 			_request_reset_counter = 0;
@@ -136,7 +137,7 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t
 
 	// ignore if rtti > 50ms
 	if (rtti > 50ll * 1000ll * 1000ll) {
-		std::cout << "\033[1;33m[ micrortps__timesync ]\tRTTI too high for timesync: " << rtti / (1000ll * 1000ll) << "ms\033[0m" << std::endl;
+		if (_debug) std::cout << "\033[1;33m[ micrortps__timesync ]\tRTTI too high for timesync: " << rtti / (1000ll * 1000ll) << "ms\033[0m" << std::endl;
 		return false;
 	}
 
@@ -167,7 +168,7 @@ void TimeSync::processTimesyncMsg(timesync_msg_t * msg) {
 
 		if (getMsgTC1(msg) > 0) {
 			if (!addMeasurement(getMsgTS1(msg), getMsgTC1(msg), getMonoRawTimeNSec())) {
-				std::cerr << "\033[1;33m[ micrortps__timesync ]\tOffset not updated\033[0m" << std::endl;
+				if (_debug) std::cerr << "\033[1;33m[ micrortps__timesync ]\tOffset not updated\033[0m" << std::endl;
 			}
 
 		} else if (getMsgTC1(msg) == 0) {
diff --git a/msg/templates/urtps/microRTPS_timesync.h.em b/msg/templates/urtps/microRTPS_timesync.h.em
index 1a1694a31a..611839b4d4 100644
--- a/msg/templates/urtps/microRTPS_timesync.h.em
+++ b/msg/templates/urtps/microRTPS_timesync.h.em
@@ -109,7 +109,7 @@ using TimesyncPublisher = timesync_Publisher;
 
 class TimeSync {
 public:
-	TimeSync();
+	TimeSync(bool debug);
 	virtual ~TimeSync();
 
 	/**
@@ -196,6 +196,8 @@ private:
 	uint8_t _last_msg_seq;
 	uint8_t _last_remote_msg_seq;
 
+	bool _debug;
+
 @[if ros2_distro]@
 	Timesync_Publisher _timesync_pub;
 	Timesync_Subscriber _timesync_sub;