From 1cbb3ebd4f5b2118cfc851ddbf1230be1b26d9d9 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Fri, 17 Jan 2020 12:03:00 +0000 Subject: [PATCH] microRTPS bridge: clean build warnings; improve verbosity --- msg/templates/urtps/Publisher.cpp.em | 7 +-- msg/templates/urtps/RtpsTopics.cpp.em | 12 +++-- msg/templates/urtps/Subscriber.cpp.em | 4 +- msg/templates/urtps/microRTPS_agent.cpp.em | 9 ++-- msg/templates/urtps/microRTPS_transport.cpp | 59 ++++++++------------- 5 files changed, 41 insertions(+), 50 deletions(-) diff --git a/msg/templates/urtps/Publisher.cpp.em b/msg/templates/urtps/Publisher.cpp.em index 2ea110d960..32513fca91 100644 --- a/msg/templates/urtps/Publisher.cpp.em +++ b/msg/templates/urtps/Publisher.cpp.em @@ -122,21 +122,22 @@ bool @(topic)_Publisher::init() mp_publisher = Domain::createPublisher(mp_participant, Wparam, static_cast(&m_listener)); if(mp_publisher == nullptr) return false; - //std::cout << "Publisher created, waiting for Subscribers." << std::endl; return true; } void @(topic)_Publisher::PubListener::onPublicationMatched(Publisher* pub, MatchingInfo& info) { + (void)pub; + if (info.status == MATCHED_MATCHING) { n_matched++; - std::cout << "Publisher matched" << std::endl; + std::cout << " - @(topic) publisher matched" << std::endl; } else { n_matched--; - std::cout << "Publisher unmatched" << std::endl; + std::cout << " - @(topic) publisher unmatched" << std::endl; } } diff --git a/msg/templates/urtps/RtpsTopics.cpp.em b/msg/templates/urtps/RtpsTopics.cpp.em index ce6c961a01..b1cb6a145b 100644 --- a/msg/templates/urtps/RtpsTopics.cpp.em +++ b/msg/templates/urtps/RtpsTopics.cpp.em @@ -65,27 +65,31 @@ bool RtpsTopics::init(std::condition_variable* t_send_queue_cv, std::mutex* t_se { @[if recv_topics]@ // Initialise subscribers + std::cout << "--- Subscribers ---" << std::endl; @[for topic in recv_topics]@ if (_@(topic)_sub.init(@(rtps_message_id(ids, topic)), t_send_queue_cv, t_send_queue_mutex, t_send_queue)) { - std::cout << "@(topic) subscriber started" << std::endl; + std::cout << "- @(topic) subscriber started" << std::endl; } else { - std::cout << "ERROR starting @(topic) subscriber" << std::endl; + std::cerr << "Failed starting @(topic) subscriber" << std::endl; return false; } @[end for]@ + std::cout << "--------------------" << std::endl << std::endl; @[end if]@ @[if send_topics]@ // Initialise publishers + std::cout << "---- Publishers ----" << std::endl; @[for topic in send_topics]@ if (_@(topic)_pub.init()) { - std::cout << "@(topic) publisher started" << std::endl; + std::cout << "- @(topic) publisher started" << std::endl; } else { - std::cout << "ERROR starting @(topic) publisher" << std::endl; + std::cerr << "ERROR starting @(topic) publisher" << std::endl; return false; } @[end for]@ + std::cout << "--------------------" << std::endl; @[end if]@ return true; } diff --git a/msg/templates/urtps/Subscriber.cpp.em b/msg/templates/urtps/Subscriber.cpp.em index 2dc127f728..51d0d12bc7 100644 --- a/msg/templates/urtps/Subscriber.cpp.em +++ b/msg/templates/urtps/Subscriber.cpp.em @@ -132,12 +132,12 @@ void @(topic)_Subscriber::SubListener::onSubscriptionMatched(Subscriber* sub, Ma if (info.status == MATCHED_MATCHING) { n_matched++; - std::cout << "Subscriber matched" << std::endl; + std::cout << " - @(topic) subscriber matched" << std::endl; } else { n_matched--; - std::cout << "Subscriber unmatched" << std::endl; + std::cout << " - @(topic) subscriber unmatched" << std::endl; } } diff --git a/msg/templates/urtps/microRTPS_agent.cpp.em b/msg/templates/urtps/microRTPS_agent.cpp.em index 48a2189668..be819d0a73 100644 --- a/msg/templates/urtps/microRTPS_agent.cpp.em +++ b/msg/templates/urtps/microRTPS_agent.cpp.em @@ -168,7 +168,7 @@ std::condition_variable t_send_queue_cv; std::mutex t_send_queue_mutex; std::queue t_send_queue; -void t_send(void *data) +void t_send(void*) { char data_buffer[BUFFER_SIZE] = {}; uint32_t length = 0; @@ -212,19 +212,22 @@ int main(int argc, char** argv) // register signal SIGINT and signal handler signal(SIGINT, signal_handler); + printf("--- MicroRTPS Agent ---\n"); + printf("- Starting link...\n"); + switch (_options.transport) { case options::eTransports::UART: { transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms); - printf("\nUART transport: device: %s; baudrate: %d; sleep: %dus; poll: %dms\n\n", + printf("- UART transport: device: %s; baudrate: %d; sleep: %dus; poll: %dms\n", _options.device, _options.baudrate, _options.sleep_us, _options.poll_ms); } break; case options::eTransports::UDP: { transport_node = new UDP_node(_options.ip, _options.recv_port, _options.send_port); - printf("\nUDP transport: ip address: %s; recv port: %u; send port: %u; sleep: %dus\n\n", + printf("- UDP transport: ip address: %s; recv port: %u; send port: %u; sleep: %dus\n", _options.ip, _options.recv_port, _options.send_port, _options.sleep_us); } break; diff --git a/msg/templates/urtps/microRTPS_transport.cpp b/msg/templates/urtps/microRTPS_transport.cpp index fd0fab4544..fbf0bb2344 100644 --- a/msg/templates/urtps/microRTPS_transport.cpp +++ b/msg/templates/urtps/microRTPS_transport.cpp @@ -150,7 +150,7 @@ ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer // Start not found if (msg_start_pos > rx_buff_pos - header_size) { #ifndef PX4_INFO - printf(" (↓↓ %u)", msg_start_pos); + printf(" (↓↓ %u)\n", msg_start_pos); #else PX4_INFO(" (↓↓ %u)", msg_start_pos); #endif /* PX4_INFO */ @@ -175,7 +175,7 @@ ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer // If there's garbage at the beginning, drop it if (msg_start_pos > 0) { #ifndef PX4_INFO - printf(" (↓ %u)", msg_start_pos); + printf(" (↓ %u)\n", msg_start_pos); #else PX4_INFO(" (↓ %u)", msg_start_pos); #endif /* PX4_INFO */ @@ -191,8 +191,8 @@ ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer if (read_crc != calc_crc) { #ifndef PX4_ERR - printf("Bad CRC %u != %u", read_crc, calc_crc); - printf(" (↓ %lu)", (unsigned long)(header_size + payload_len)); + printf("Bad CRC %u != %u\n", read_crc, calc_crc); + printf(" (↓ %lu)\n", (unsigned long)(header_size + payload_len)); #else PX4_ERR("Bad CRC %u != %u", read_crc, calc_crc); PX4_ERR(" (↓ %lu)", (unsigned long)(header_size + payload_len)); @@ -224,17 +224,7 @@ ssize_t Transport_node::write(const uint8_t topic_ID, char buffer[], size_t leng return -1; } - static struct Header header = { - .marker = {'>', '>', '>'}, - .topic_ID = 0u, - .seq = 0u, - .payload_len_h = 0u, - .payload_len_l = 0u, - .crc_h = 0u, - .crc_l = 0u - - }; - + static struct Header header = {{'>', '>', '>'}, 0u, 0u, 0u, 0u, 0u, 0u}; static uint8_t seq = 0; // [>,>,>,topic_ID,seq,payload_length,CRCHigh,CRCLow,payload_start, ... ,payload_end] @@ -252,16 +242,9 @@ ssize_t Transport_node::write(const uint8_t topic_ID, char buffer[], size_t leng memcpy(buffer, &header, sizeof(header)); ssize_t len = node_write(buffer, length + sizeof(header)); if (len != ssize_t(length + sizeof(header))) { - goto err; + return len; } return len + sizeof(header); - -err: - //int errsv = errno; - //if (len == -1 ) PX4_ERR(" => Writing error '%d'", errsv); - //else PX4_ERR(" => Wrote '%ld' != length(%lu) error '%d'", (long)len, (unsigned long)length, errsv); - - return len; } UART_node::UART_node(const char *_uart_name, uint32_t _baudrate, uint32_t _poll_ms): @@ -287,7 +270,7 @@ int UART_node::init() if (uart_fd < 0) { #ifndef PX4_ERR - printf("Failed to open device: %s (%d)", uart_name, errno); + printf("Failed to open device: %s (%d)\n", uart_name, errno); #else PX4_ERR("Failed to open device: %s (%d)", uart_name, errno); #endif /* PX4_ERR */ @@ -307,7 +290,7 @@ int UART_node::init() if ((termios_state = tcgetattr(uart_fd, &uart_config)) < 0) { int errno_bkp = errno; #ifndef PX4_ERR - printf("ERR GET CONF %s: %d (%d)", uart_name, termios_state, errno); + printf("ERR GET CONF %s: %d (%d)\n", uart_name, termios_state, errno); #else PX4_ERR("ERR GET CONF %s: %d (%d)", uart_name, termios_state, errno); #endif /* PX4_ERR */ @@ -347,7 +330,7 @@ int UART_node::init() if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { int errno_bkp = errno; #ifndef PX4_ERR - printf("ERR SET BAUD %s: %d (%d)", uart_name, termios_state, errno); + printf("ERR SET BAUD %s: %d (%d)\n", uart_name, termios_state, errno); #else PX4_ERR("ERR SET BAUD %s: %d (%d)", uart_name, termios_state, errno); #endif /* PX4_ERR */ @@ -359,7 +342,7 @@ int UART_node::init() if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) { int errno_bkp = errno; #ifndef PX4_ERR - printf("ERR SET CONF %s (%d)", uart_name, errno); + printf("ERR SET CONF %s (%d)\n", uart_name, errno); #else PX4_ERR("ERR SET CONF %s (%d)", uart_name, errno); #endif /* PX4_ERR */ @@ -385,13 +368,13 @@ int UART_node::init() if (flush) { #ifndef PX4_INFO - printf("Flush"); + printf("Flush\n"); #else PX4_INFO("Flush"); #endif /* PX4_INFO */ } else { #ifndef PX4_INFO - printf("No flush"); + printf("No flush\n"); #else PX4_INFO("No flush"); #endif /* PX4_INFO */ @@ -412,7 +395,7 @@ uint8_t UART_node::close() { if (-1 != uart_fd) { #ifndef PX4_WARN - printf("Closed UART..."); + printf("Closed UART...\n"); #else PX4_WARN("Closed UART..."); #endif /* PX4_WARN */ @@ -572,28 +555,28 @@ int UDP_node::init_receiver(uint16_t udp_port) if ((receiver_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { #ifndef PX4_ERR - printf("Create socket failed"); + printf("Create socket failed\n"); #else PX4_ERR("Create socket failed"); #endif /* PX4_ERR */ return -1; } #ifndef PX4_INFO - printf("Trying to connect..."); + printf("- Trying to connect..."); #else PX4_INFO("Trying to connect..."); #endif /* PX4_INFO */ if (bind(receiver_fd, (struct sockaddr *)&receiver_inaddr, sizeof(receiver_inaddr)) < 0) { #ifndef PX4_ERR - printf("Bind failed"); + printf("Bind failed\n"); #else PX4_ERR("Bind failed"); #endif /* PX4_ERR */ return -1; } #ifndef PX4_INFO - printf("Connected to server!"); + printf("Connected to server!\n\n"); #else PX4_INFO("Connected to server!"); #endif /* PX4_INFO */ @@ -607,7 +590,7 @@ int UDP_node::init_sender(uint16_t udp_port) if ((sender_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { #ifndef PX4_ERR - printf("Create socket failed"); + printf("Create socket failed\n"); #else PX4_ERR("Create socket failed"); #endif /* PX4_ERR */ @@ -620,7 +603,7 @@ int UDP_node::init_sender(uint16_t udp_port) if (inet_aton(udp_ip, &sender_outaddr.sin_addr) == 0) { #ifndef PX4_ERR - printf("inet_aton() failed"); + printf("inet_aton() failed\n"); #else PX4_ERR("inet_aton() failed"); #endif /* PX4_ERR */ @@ -638,7 +621,7 @@ uint8_t UDP_node::close() if (sender_fd != -1) { #ifndef PX4_WARN - printf("Closed sender socket!"); + printf("Closed sender socket!\n"); #else PX4_WARN("Closed sender socket!"); #endif /* PX4_WARN */ @@ -649,7 +632,7 @@ uint8_t UDP_node::close() if (receiver_fd != -1) { #ifndef PX4_WARN - printf("Closed receiver socket!"); + printf("Closed receiver socket!\n"); #else PX4_WARN("Closed receiver socket!"); #endif /* PX4_WARN */