microRTPS: client: show diagnostic of current bandwidth usage on the 'status' option

msg/templates/uorb_microcdr/microRTPS_client.cpp.em

@@ -79,6 +79,12 @@ receive_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] =
 uint8_t last_remote_msg_seq = 0;
 uint8_t last_msg_seq = 0;
 
+uint64_t tx_last_sec_read = 0;
+uint64_t rx_last_sec_read = 0;
+
+pthread_mutex_t tx_lock;
+pthread_mutex_t rx_lock;
+
 @[if recv_topics]@
 // Publishers for received messages
 struct RcvTopicsPubs {
@@ -101,10 +107,12 @@ struct SendTopicsSubs {
 
 struct SendThreadArgs {
 	uint64_t &total_sent;
+	uint64_t &sent_last_sec;
 	uint64_t &sent;
 	int &sent_loop;
-	SendThreadArgs(uint64_t &total_sent_, uint64_t &sent_, int &sent_loop_)
+	SendThreadArgs(uint64_t &total_sent_, uint64_t &sent_last_sec_, uint64_t &sent_, int &sent_loop_)
 		: total_sent(total_sent_),
+		  sent_last_sec(sent_last_sec_),
 		  sent(sent_),
 		  sent_loop(sent_loop_) {}
 };
@@ -123,6 +131,14 @@ void *send(void *args)
 	header_length = transport_node->get_header_length();
 	ucdr_init_buffer(&writer, reinterpret_cast<uint8_t *>(&data_buffer[header_length]), BUFFER_SIZE - header_length);
 
+	pthread_t tx_per_second_thread;
+	int rc = pthread_create(&tx_per_second_thread, nullptr, &tx_per_second, (void *)&data->sent_last_sec);
+	if (rc != 0) {
+		errno = rc;
+		PX4_ERR("Could not create tx counter thread (%d)", errno);
+		return nullptr;
+	}
+
 	while (!_should_exit_task) {
 @[    for idx, topic in enumerate(send_topics)]@
 		{
@@ -147,6 +163,9 @@ void *send(void *args)
 
 					if (0 < (read = transport_node->write(static_cast<char>(@(rtps_message_id(ids, topic))), data_buffer, length))) {
 						data->total_sent += read;
+						pthread_mutex_lock(&tx_lock);
+						tx_last_sec_read += read;
+						pthread_mutex_unlock(&tx_lock);
 						++data->sent;
 					}
 
@@ -161,11 +180,36 @@ void *send(void *args)
 		++data->sent_loop;
 	}
 
+	pthread_join(tx_per_second_thread, nullptr);
 	delete subs;
 
 	return nullptr;
 }
 
+void *tx_per_second(void *sent_last_sec)
+{
+	while (!_should_exit_task) {
+		pthread_mutex_lock(&tx_lock);
+		*((uint64_t *) sent_last_sec) = tx_last_sec_read;
+		tx_last_sec_read = 0;
+		pthread_mutex_unlock(&tx_lock);
+		px4_sleep(1);
+	}
+	return nullptr;
+}
+
+void *rx_per_second(void *rcvd_last_sec)
+{
+	while (!_should_exit_task) {
+		pthread_mutex_lock(&rx_lock);
+		*((uint64_t *) rcvd_last_sec) = rx_last_sec_read;
+		rx_last_sec_read = 0;
+		pthread_mutex_unlock(&rx_lock);
+		px4_sleep(1);
+	}
+	return nullptr;
+}
+
 static int launch_send_thread(pthread_t &sender_thread, struct SendThreadArgs &args)
 {
 	pthread_attr_t sender_thread_attr;
@@ -192,8 +236,8 @@ static int launch_send_thread(pthread_t &sender_thread, struct SendThreadArgs &a
 }
 @[end if]@
 
-void micrortps_start_topics(struct timespec &begin, uint64_t &total_read, uint64_t &total_sent, uint64_t &received,
+void micrortps_start_topics(struct timespec &begin, uint64_t &total_rcvd, uint64_t &total_sent, uint64_t &sent_last_sec,
-			    uint64_t &sent, int &rcvd_loop, int &sent_loop)
+			    uint64_t &rcvd_last_sec, uint64_t &received, uint64_t &sent, int &rcvd_loop, int &sent_loop)
 {
 @[if recv_topics]@
 	char data_buffer[BUFFER_SIZE] = {};
@@ -208,13 +252,21 @@ void micrortps_start_topics(struct timespec &begin, uint64_t &total_read, uint64
 	// ucdrBuffer to deserialize using the user defined buffer
 	ucdrBuffer reader;
 	ucdr_init_buffer(&reader, reinterpret_cast<uint8_t *>(data_buffer), BUFFER_SIZE);
+
+	pthread_t rx_per_second_thread;
+	int rc = pthread_create(&rx_per_second_thread, nullptr, &rx_per_second, (void *)&rcvd_last_sec);
+	if (rc != 0) {
+		errno = rc;
+		PX4_ERR("Could not create rx counter thread (%d)", errno);
+		return;
+	}
 @[end if]@
 
 	px4_clock_gettime(CLOCK_REALTIME, &begin);
 	_should_exit_task = false;
 @[if send_topics]@
 	// var struct to be updated on the thread
-	SendThreadArgs *sender_thread_args = new SendThreadArgs(total_sent, sent, sent_loop);
+	SendThreadArgs *sender_thread_args = new SendThreadArgs(total_sent, sent_last_sec, sent, sent_loop);
 
 	// create a thread for sending data
 	pthread_t sender_thread;
@@ -224,7 +276,11 @@ void micrortps_start_topics(struct timespec &begin, uint64_t &total_read, uint64
 	while (!_should_exit_task) {
 @[if recv_topics]@
 		while (0 < (read = transport_node->read(&topic_ID, data_buffer, BUFFER_SIZE))) {
-			total_read += read;
+			total_rcvd += read;
+			pthread_mutex_lock(&rx_lock);
+			rcvd_last_sec += read;
+			pthread_mutex_unlock(&rx_lock);
+
 			uint64_t read_time = hrt_absolute_time();
 
 			switch (topic_ID) {
@@ -258,12 +314,13 @@ void micrortps_start_topics(struct timespec &begin, uint64_t &total_read, uint64
 		++rcvd_loop;
 	}
 
-@[if recv_topics]@
-	delete pubs;
-@[end if]@
 @[if send_topics]@
 	delete sender_thread_args;
 	_should_exit_task = true;
 	pthread_join(sender_thread, nullptr);
 @[end if]@
+@[if recv_topics]@
+	pthread_join(rx_per_second_thread, nullptr);
+	delete pubs;
+@[end if]@
 }

src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h

@@ -1,7 +1,7 @@
 /****************************************************************************
  *
  * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
- * Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -58,9 +58,13 @@
 #define DEFAULT_RECV_PORT 2019
 #define DEFAULT_SEND_PORT 2020
 
+
 void *send(void *args);
-void micrortps_start_topics(struct timespec &begin, uint64_t &total_read, uint64_t &total_sent, uint64_t &received,
+void *tx_per_second(void *sent_last_sec);
-			    uint64_t &sent, int &rcvd_loop, int &sent_loop);
+void *rx_per_second(void *rcvd_last_sec);
+
+void micrortps_start_topics(struct timespec &begin, uint64_t &total_rcvd, uint64_t &total_sent, uint64_t &sent_last_sec,
+			    uint64_t &rcvd_last_sec, uint64_t &received, uint64_t &sent, int &rcvd_loop, int &sent_loop);
 
 struct baudtype {
 	speed_t code;

src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp

@@ -1,7 +1,7 @@
 /****************************************************************************
  *
  * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
- * Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -58,8 +58,10 @@ struct options _options;
 struct timespec begin;
 struct timespec end;
 
-uint64_t total_read{0};
+uint64_t total_rcvd{0};
 uint64_t total_sent{0};
+uint64_t rcvd_last_sec{0};
+uint64_t sent_last_sec{0};
 uint64_t received{0};
 uint64_t sent{0};
 int rcv_loop{0};
@@ -189,14 +191,15 @@ static int micrortps_start(int argc, char *argv[])
 		return -1;
 	}
 
-	micrortps_start_topics(begin, total_read, total_sent, received, sent, rcv_loop, send_loop);
+	micrortps_start_topics(begin, total_rcvd, total_sent, sent_last_sec, rcvd_last_sec, received, sent, rcv_loop,
+			       send_loop);
 
 	px4_clock_gettime(CLOCK_REALTIME, &end);
 
 	const double elapsed_secs = static_cast<double>(end.tv_sec - begin.tv_sec + (end.tv_nsec - begin.tv_nsec) / 1e9);
 
 	PX4_INFO("RECEIVED: %" PRIu64 " messages in %d LOOPS, %" PRIu64 " bytes in %.03f seconds - avg %.02fKB/s",
-		 received, rcv_loop, total_read, elapsed_secs, static_cast<double>(total_read / (1e3 * elapsed_secs)));
+		 received, rcv_loop, total_rcvd, elapsed_secs, static_cast<double>(total_rcvd / (1e3 * elapsed_secs)));
 	PX4_INFO("SENT: %" PRIu64 " messages in %d LOOPS, %" PRIu64 " bytes in %.03f seconds - avg %.02fKB/s",
 		 sent, send_loop, total_sent, elapsed_secs, total_sent / (1e3 * elapsed_secs));
 
@@ -254,10 +257,12 @@ int micrortps_client_main(int argc, char *argv[])
 			printf("\tup and running for %.03f seconds\n", elapsed_secs);
 			printf("\tnr. of messages received: %" PRIu64 "\n", received);
 			printf("\tnr. of messages sent: %" PRIu64 "\n", sent);
-			printf("\ttotal data read: %" PRIu64 " bytes\n", total_read);
+			printf("\ttotal data read: %" PRIu64 " bytes\n", total_rcvd);
 			printf("\ttotal data sent: %" PRIu64 " bytes\n", total_sent);
 			printf("\trates:\n");
-			printf("\t  avg rx: %.3f kB/s\n", static_cast<double>(total_read / (1e3 * elapsed_secs)));
+			printf("\t  rx: %.3f kB/s\n", rcvd_last_sec / 1E3);
+			printf("\t  tx: %.3f kB/s\n", sent_last_sec / 1E3);
+			printf("\t  avg rx: %.3f kB/s\n", static_cast<double>(total_rcvd / (1e3 * elapsed_secs)));
 			printf("\t  avg tx: %.3f kB/s\n", static_cast<double>(total_sent / (1e3 * elapsed_secs)));
 		}