Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 03:49:12 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

microRTPS bridge: clean build warnings; improve verbosity

Browse Source
This commit is contained in:
TSC21
2020-01-17 12:03:00 +00:00
committed by Nuno Marques
parent 3387c9599c
commit 1cbb3ebd4f
5 changed files with 41 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/templates/urtps/Publisher.cpp.em
+4 -3
View File
@@ -122,21 +122,22 @@ bool @(topic)_Publisher::init()
mp_publisher = Domain::createPublisher(mp_participant, Wparam, static_cast<PublisherListener*>(&m_listener)); mp_publisher = Domain::createPublisher(mp_participant, Wparam, static_cast<PublisherListener*>(&m_listener));
if(mp_publisher == nullptr) if(mp_publisher == nullptr)
return false; return false;
//std::cout << "Publisher created, waiting for Subscribers." << std::endl;
return true; return true;
} }
void @(topic)_Publisher::PubListener::onPublicationMatched(Publisher* pub, MatchingInfo& info) void @(topic)_Publisher::PubListener::onPublicationMatched(Publisher* pub, MatchingInfo& info)
{ {
(void)pub;
if (info.status == MATCHED_MATCHING) if (info.status == MATCHED_MATCHING)
{ {
n_matched++; n_matched++;
std::cout << "Publisher matched" << std::endl; std::cout << " - @(topic) publisher matched" << std::endl;
} }
else else
{ {
n_matched--; n_matched--;
std::cout << "Publisher unmatched" << std::endl; std::cout << " - @(topic) publisher unmatched" << std::endl;
} }
} }
msg/templates/urtps/RtpsTopics.cpp.em
+8 -4
View File
@@ -65,27 +65,31 @@ bool RtpsTopics::init(std::condition_variable* t_send_queue_cv, std::mutex* t_se
{ {
@[if recv_topics]@ @[if recv_topics]@
// Initialise subscribers // Initialise subscribers
std::cout << "--- Subscribers ---" << std::endl;
@[for topic in recv_topics]@ @[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)) { 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 { } else {
std::cout << "ERROR starting @(topic) subscriber" << std::endl; std::cerr << "Failed starting @(topic) subscriber" << std::endl;
return false; return false;
} }
@[end for]@ @[end for]@
std::cout << "--------------------" << std::endl << std::endl;
@[end if]@ @[end if]@
@[if send_topics]@ @[if send_topics]@
// Initialise publishers // Initialise publishers
std::cout << "---- Publishers ----" << std::endl;
@[for topic in send_topics]@ @[for topic in send_topics]@
if (_@(topic)_pub.init()) { if (_@(topic)_pub.init()) {
std::cout << "@(topic) publisher started" << std::endl; std::cout << "- @(topic) publisher started" << std::endl;
} else { } else {
std::cout << "ERROR starting @(topic) publisher" << std::endl; std::cerr << "ERROR starting @(topic) publisher" << std::endl;
return false; return false;
} }
@[end for]@ @[end for]@
std::cout << "--------------------" << std::endl;
@[end if]@ @[end if]@
return true; return true;
} }
msg/templates/urtps/Subscriber.cpp.em
+2 -2
View File
@@ -132,12 +132,12 @@ void @(topic)_Subscriber::SubListener::onSubscriptionMatched(Subscriber* sub, Ma
if (info.status == MATCHED_MATCHING) if (info.status == MATCHED_MATCHING)
{ {
n_matched++; n_matched++;
std::cout << "Subscriber matched" << std::endl; std::cout << " - @(topic) subscriber matched" << std::endl;
} }
else else
{ {
n_matched--; n_matched--;
std::cout << "Subscriber unmatched" << std::endl; std::cout << " - @(topic) subscriber unmatched" << std::endl;
} }
} }
msg/templates/urtps/microRTPS_agent.cpp.em
+6 -3
View File
@@ -168,7 +168,7 @@ std::condition_variable t_send_queue_cv;
std::mutex t_send_queue_mutex; std::mutex t_send_queue_mutex;
std::queue<uint8_t> t_send_queue; std::queue<uint8_t> t_send_queue;
void t_send(void *data) void t_send(void*)
{ {
char data_buffer[BUFFER_SIZE] = {}; char data_buffer[BUFFER_SIZE] = {};
uint32_t length = 0; uint32_t length = 0;
@@ -212,19 +212,22 @@ int main(int argc, char** argv)
// register signal SIGINT and signal handler // register signal SIGINT and signal handler
signal(SIGINT, signal_handler); signal(SIGINT, signal_handler);
printf("--- MicroRTPS Agent ---\n");
printf("- Starting link...\n");
switch (_options.transport) switch (_options.transport)
{ {
case options::eTransports::UART: case options::eTransports::UART:
{ {
transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms); 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); _options.device, _options.baudrate, _options.sleep_us, _options.poll_ms);
} }
break; break;
case options::eTransports::UDP: case options::eTransports::UDP:
{ {
transport_node = new UDP_node(_options.ip, _options.recv_port, _options.send_port); 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); _options.ip, _options.recv_port, _options.send_port, _options.sleep_us);
} }
break; break;
msg/templates/urtps/microRTPS_transport.cpp
+21 -38
View File
@@ -150,7 +150,7 @@ ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer
// Start not found // Start not found
if (msg_start_pos > rx_buff_pos - header_size) { if (msg_start_pos > rx_buff_pos - header_size) {
#ifndef PX4_INFO #ifndef PX4_INFO
printf(" (↓↓ %u)", msg_start_pos); printf(" (↓↓ %u)\n", msg_start_pos);
#else #else
PX4_INFO(" (↓↓ %u)", msg_start_pos); PX4_INFO(" (↓↓ %u)", msg_start_pos);
#endif /* PX4_INFO */ #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 there's garbage at the beginning, drop it
if (msg_start_pos > 0) { if (msg_start_pos > 0) {
#ifndef PX4_INFO #ifndef PX4_INFO
printf(" (↓ %u)", msg_start_pos); printf(" (↓ %u)\n", msg_start_pos);
#else #else
PX4_INFO(" (↓ %u)", msg_start_pos); PX4_INFO(" (↓ %u)", msg_start_pos);
#endif /* PX4_INFO */ #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) { if (read_crc != calc_crc) {
#ifndef PX4_ERR #ifndef PX4_ERR
printf("Bad CRC %u != %u", read_crc, calc_crc); printf("Bad CRC %u != %u\n", read_crc, calc_crc);
printf(" (↓ %lu)", (unsigned long)(header_size + payload_len)); printf(" (↓ %lu)\n", (unsigned long)(header_size + payload_len));
#else #else
PX4_ERR("Bad CRC %u != %u", read_crc, calc_crc); PX4_ERR("Bad CRC %u != %u", read_crc, calc_crc);
PX4_ERR(" (↓ %lu)", (unsigned long)(header_size + payload_len)); 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; return -1;
} }
static struct Header header = { static struct Header header = {{'>', '>', '>'}, 0u, 0u, 0u, 0u, 0u, 0u};
.marker = {'>', '>', '>'},
.topic_ID = 0u,
.seq = 0u,
.payload_len_h = 0u,
.payload_len_l = 0u,
.crc_h = 0u,
.crc_l = 0u
};
static uint8_t seq = 0; static uint8_t seq = 0;
// [>,>,>,topic_ID,seq,payload_length,CRCHigh,CRCLow,payload_start, ... ,payload_end] // [>,>,>,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)); memcpy(buffer, &header, sizeof(header));
ssize_t len = node_write(buffer, length + sizeof(header)); ssize_t len = node_write(buffer, length + sizeof(header));
if (len != ssize_t(length + sizeof(header))) { if (len != ssize_t(length + sizeof(header))) {
goto err; return len;
} }
return len + sizeof(header); 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): 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) { if (uart_fd < 0) {
#ifndef PX4_ERR #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 #else
PX4_ERR("Failed to open device: %s (%d)", uart_name, errno); PX4_ERR("Failed to open device: %s (%d)", uart_name, errno);
#endif /* PX4_ERR */ #endif /* PX4_ERR */
@@ -307,7 +290,7 @@ int UART_node::init()
if ((termios_state = tcgetattr(uart_fd, &uart_config)) < 0) { if ((termios_state = tcgetattr(uart_fd, &uart_config)) < 0) {
int errno_bkp = errno; int errno_bkp = errno;
#ifndef PX4_ERR #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 #else
PX4_ERR("ERR GET CONF %s: %d (%d)", uart_name, termios_state, errno); PX4_ERR("ERR GET CONF %s: %d (%d)", uart_name, termios_state, errno);
#endif /* PX4_ERR */ #endif /* PX4_ERR */
@@ -347,7 +330,7 @@ int UART_node::init()
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
int errno_bkp = errno; int errno_bkp = errno;
#ifndef PX4_ERR #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 #else
PX4_ERR("ERR SET BAUD %s: %d (%d)", uart_name, termios_state, errno); PX4_ERR("ERR SET BAUD %s: %d (%d)", uart_name, termios_state, errno);
#endif /* PX4_ERR */ #endif /* PX4_ERR */
@@ -359,7 +342,7 @@ int UART_node::init()
if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) { if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) {
int errno_bkp = errno; int errno_bkp = errno;
#ifndef PX4_ERR #ifndef PX4_ERR
printf("ERR SET CONF %s (%d)", uart_name, errno); printf("ERR SET CONF %s (%d)\n", uart_name, errno);
#else #else
PX4_ERR("ERR SET CONF %s (%d)", uart_name, errno); PX4_ERR("ERR SET CONF %s (%d)", uart_name, errno);
#endif /* PX4_ERR */ #endif /* PX4_ERR */
@@ -385,13 +368,13 @@ int UART_node::init()
if (flush) { if (flush) {
#ifndef PX4_INFO #ifndef PX4_INFO
printf("Flush"); printf("Flush\n");
#else #else
PX4_INFO("Flush"); PX4_INFO("Flush");
#endif /* PX4_INFO */ #endif /* PX4_INFO */
} else { } else {
#ifndef PX4_INFO #ifndef PX4_INFO
printf("No flush"); printf("No flush\n");
#else #else
PX4_INFO("No flush"); PX4_INFO("No flush");
#endif /* PX4_INFO */ #endif /* PX4_INFO */
@@ -412,7 +395,7 @@ uint8_t UART_node::close()
{ {
if (-1 != uart_fd) { if (-1 != uart_fd) {
#ifndef PX4_WARN #ifndef PX4_WARN
printf("Closed UART..."); printf("Closed UART...\n");
#else #else
PX4_WARN("Closed UART..."); PX4_WARN("Closed UART...");
#endif /* PX4_WARN */ #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) { if ((receiver_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
#ifndef PX4_ERR #ifndef PX4_ERR
printf("Create socket failed"); printf("Create socket failed\n");
#else #else
PX4_ERR("Create socket failed"); PX4_ERR("Create socket failed");
#endif /* PX4_ERR */ #endif /* PX4_ERR */
return -1; return -1;
} }
#ifndef PX4_INFO #ifndef PX4_INFO
printf("Trying to connect..."); printf("- Trying to connect...");
#else #else
PX4_INFO("Trying to connect..."); PX4_INFO("Trying to connect...");
#endif /* PX4_INFO */ #endif /* PX4_INFO */
if (bind(receiver_fd, (struct sockaddr *)&receiver_inaddr, sizeof(receiver_inaddr)) < 0) { if (bind(receiver_fd, (struct sockaddr *)&receiver_inaddr, sizeof(receiver_inaddr)) < 0) {
#ifndef PX4_ERR #ifndef PX4_ERR
printf("Bind failed"); printf("Bind failed\n");
#else #else
PX4_ERR("Bind failed"); PX4_ERR("Bind failed");
#endif /* PX4_ERR */ #endif /* PX4_ERR */
return -1; return -1;
} }
#ifndef PX4_INFO #ifndef PX4_INFO
printf("Connected to server!"); printf("Connected to server!\n\n");
#else #else
PX4_INFO("Connected to server!"); PX4_INFO("Connected to server!");
#endif /* PX4_INFO */ #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) { if ((sender_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
#ifndef PX4_ERR #ifndef PX4_ERR
printf("Create socket failed"); printf("Create socket failed\n");
#else #else
PX4_ERR("Create socket failed"); PX4_ERR("Create socket failed");
#endif /* PX4_ERR */ #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) { if (inet_aton(udp_ip, &sender_outaddr.sin_addr) == 0) {
#ifndef PX4_ERR #ifndef PX4_ERR
printf("inet_aton() failed"); printf("inet_aton() failed\n");
#else #else
PX4_ERR("inet_aton() failed"); PX4_ERR("inet_aton() failed");
#endif /* PX4_ERR */ #endif /* PX4_ERR */
@@ -638,7 +621,7 @@ uint8_t UDP_node::close()
if (sender_fd != -1) { if (sender_fd != -1) {
#ifndef PX4_WARN #ifndef PX4_WARN
printf("Closed sender socket!"); printf("Closed sender socket!\n");
#else #else
PX4_WARN("Closed sender socket!"); PX4_WARN("Closed sender socket!");
#endif /* PX4_WARN */ #endif /* PX4_WARN */
@@ -649,7 +632,7 @@ uint8_t UDP_node::close()
if (receiver_fd != -1) { if (receiver_fd != -1) {
#ifndef PX4_WARN #ifndef PX4_WARN
printf("Closed receiver socket!"); printf("Closed receiver socket!\n");
#else #else
PX4_WARN("Closed receiver socket!"); PX4_WARN("Closed receiver socket!");
#endif /* PX4_WARN */ #endif /* PX4_WARN */