From 0cc79f3e4838cb0bca58ee5f14422a076a660348 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Thu, 6 May 2021 17:36:18 +0200 Subject: [PATCH] microRTPS: client: show diagnostic of current bandwidth usage on the 'status' option --- .../uorb_microcdr/microRTPS_client.cpp.em | 73 +++++++++++++++++-- .../micrortps_client/microRTPS_client.h | 10 ++- .../microRTPS_client_main.cpp | 17 +++-- 3 files changed, 83 insertions(+), 17 deletions(-) diff --git a/msg/templates/uorb_microcdr/microRTPS_client.cpp.em b/msg/templates/uorb_microcdr/microRTPS_client.cpp.em index 7aa5b28410..f874cba3a4 100644 --- a/msg/templates/uorb_microcdr/microRTPS_client.cpp.em +++ b/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(&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(@(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, - uint64_t &sent, int &rcvd_loop, int &sent_loop) +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) { @[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(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]@ } diff --git a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h index 37bb518ca5..d7b76e1bd6 100644 --- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h +++ b/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, - uint64_t &sent, int &rcvd_loop, int &sent_loop); +void *tx_per_second(void *sent_last_sec); +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; diff --git a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp index cc76f28677..3c4e77c788 100644 --- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp +++ b/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(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(total_read / (1e3 * elapsed_secs))); + received, rcv_loop, total_rcvd, elapsed_secs, static_cast(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(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(total_rcvd / (1e3 * elapsed_secs))); printf("\t avg tx: %.3f kB/s\n", static_cast(total_sent / (1e3 * elapsed_secs))); }