diff --git a/msg/templates/uorb_microcdr/microRTPS_client.cpp.em b/msg/templates/uorb_microcdr/microRTPS_client.cpp.em index 57ed5aca78..7aa5b28410 100644 --- a/msg/templates/uorb_microcdr/microRTPS_client.cpp.em +++ b/msg/templates/uorb_microcdr/microRTPS_client.cpp.em @@ -76,8 +76,6 @@ receive_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] = #include @[end for]@ -void *send(void *data); - uint8_t last_remote_msg_seq = 0; uint8_t last_msg_seq = 0; @@ -100,13 +98,24 @@ struct SendTopicsSubs { @[end if]@ @[if send_topics]@ -void *send(void * /*unused*/) + +struct SendThreadArgs { + uint64_t &total_sent; + uint64_t &sent; + int &sent_loop; + SendThreadArgs(uint64_t &total_sent_, uint64_t &sent_, int &sent_loop_) + : total_sent(total_sent_), + sent(sent_), + sent_loop(sent_loop_) {} +}; + +void *send(void *args) { char data_buffer[BUFFER_SIZE] = {}; - uint64_t sent = 0, total_sent = 0; - int loop = 0, read = 0; + int read = 0; uint32_t length = 0; size_t header_length = 0; + struct SendThreadArgs *data = reinterpret_cast(args); SendTopicsSubs *subs = new SendTopicsSubs(); // ucdrBuffer to serialize using the user defined buffer @@ -114,9 +123,6 @@ void *send(void * /*unused*/) header_length = transport_node->get_header_length(); ucdr_init_buffer(&writer, reinterpret_cast(&data_buffer[header_length]), BUFFER_SIZE - header_length); - struct timespec begin; - px4_clock_gettime(CLOCK_REALTIME, &begin); - while (!_should_exit_task) { @[ for idx, topic in enumerate(send_topics)]@ { @@ -140,8 +146,8 @@ void *send(void * /*unused*/) serialize_@(send_base_types[idx])(&writer, &@(topic)_data, &data_buffer[header_length], &length); if (0 < (read = transport_node->write(static_cast(@(rtps_message_id(ids, topic))), data_buffer, length))) { - total_sent += read; - ++sent; + data->total_sent += read; + ++data->sent; } @[ if topic == 'Timesync' or topic == 'timesync']@ @@ -152,24 +158,15 @@ void *send(void * /*unused*/) } @[ end for]@ px4_usleep(_options.sleep_ms * 1000); - ++loop; + ++data->sent_loop; } - struct timespec end; - - px4_clock_gettime(CLOCK_REALTIME, &end); - - double elapsed_secs = end.tv_sec - begin.tv_sec + (end.tv_nsec - begin.tv_nsec) / 1e9; - - PX4_INFO("SENT: %" PRIu64 " messages in %d LOOPS, %" PRIu64 " bytes in %.03f seconds - %.02fKB/s", - sent, loop, total_sent, elapsed_secs, total_sent / (1e3 * elapsed_secs)); - delete subs; return nullptr; } -static int launch_send_thread(pthread_t &sender_thread) +static int launch_send_thread(pthread_t &sender_thread, struct SendThreadArgs &args) { pthread_attr_t sender_thread_attr; pthread_attr_init(&sender_thread_attr); @@ -178,12 +175,16 @@ static int launch_send_thread(pthread_t &sender_thread) (void)pthread_attr_getschedparam(&sender_thread_attr, ¶m); param.sched_priority = SCHED_PRIORITY_DEFAULT; (void)pthread_attr_setschedparam(&sender_thread_attr, ¶m); - pthread_create(&sender_thread, &sender_thread_attr, send, nullptr); - int rc = pthread_setname_np(sender_thread, "urtpsclient_snd"); - if (rc != 0) - { + int rc = pthread_create(&sender_thread, &sender_thread_attr, &send, (void *)&args); + if (rc != 0) { errno = rc; - PX4_ERR("Could not set pthread name (%d)", errno); + PX4_ERR("Could not create send thread (%d)", errno); + return -1; + } + rc = pthread_setname_np(sender_thread, "micrortps_client_send"); + if (pthread_setname_np(sender_thread, "micrortps_client_send")) { + errno = rc; + PX4_ERR("Could not set pthread name for the send thread (%d)", errno); } pthread_attr_destroy(&sender_thread_attr); @@ -191,7 +192,8 @@ static int launch_send_thread(pthread_t &sender_thread) } @[end if]@ -void micrortps_start_topics(struct timespec &begin, uint64_t &total_read, uint64_t &received, int &loop) +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) { @[if recv_topics]@ char data_buffer[BUFFER_SIZE] = {}; @@ -211,10 +213,12 @@ void micrortps_start_topics(struct timespec &begin, uint64_t &total_read, uint64 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); - // create a thread for sending data to the simulator + // create a thread for sending data pthread_t sender_thread; - launch_send_thread(sender_thread); + launch_send_thread(sender_thread, (*sender_thread_args)); @[end if]@ while (!_should_exit_task) { @@ -248,16 +252,17 @@ void micrortps_start_topics(struct timespec &begin, uint64_t &total_read, uint64 @[end if]@ // loop forever if informed loop number is negative - if (_options.loops >= 0 && loop >= _options.loops) { break; } + if (_options.loops >= 0 && rcvd_loop >= _options.loops) { break; } px4_usleep(_options.sleep_ms * 1000); - ++loop; + ++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]@ diff --git a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h index aa0cce5493..37bb518ca5 100644 --- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h +++ b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h @@ -58,8 +58,9 @@ #define DEFAULT_RECV_PORT 2019 #define DEFAULT_SEND_PORT 2020 -void *send(void *data); -void micrortps_start_topics(struct timespec &begin, uint64_t &total_read, uint64_t &received, int &loop); +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); 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 edc9432c00..cc76f28677 100644 --- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp +++ b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp @@ -55,6 +55,16 @@ bool _should_exit_task = false; Transport_node *transport_node = nullptr; struct options _options; +struct timespec begin; +struct timespec end; + +uint64_t total_read{0}; +uint64_t total_sent{0}; +uint64_t received{0}; +uint64_t sent{0}; +int rcv_loop{0}; +int send_loop{0}; + static void usage(const char *name) { PRINT_MODULE_USAGE_NAME("micrortps_client", "communication"); @@ -179,22 +189,16 @@ static int micrortps_start(int argc, char *argv[]) return -1; } - struct timespec begin; - - struct timespec end; - - uint64_t total_read = 0, received = 0; - - int loop = 0; - - micrortps_start_topics(begin, total_read, received, loop); + micrortps_start_topics(begin, total_read, total_sent, received, sent, rcv_loop, send_loop); px4_clock_gettime(CLOCK_REALTIME, &end); - double elapsed_secs = static_cast(end.tv_sec - begin.tv_sec + (end.tv_nsec - begin.tv_nsec) / 1e9); + 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 - %.02fKB/s", - received, loop, total_read, elapsed_secs, static_cast(total_read / (1e3 * elapsed_secs))); + 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))); + 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)); delete transport_node; @@ -243,7 +247,18 @@ int micrortps_client_main(int argc, char *argv[]) PX4_INFO("Not running"); } else { - PX4_INFO("Running"); + 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); + + 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 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 avg tx: %.3f kB/s\n", static_cast(total_sent / (1e3 * elapsed_secs))); } return 0;