From 13a3791c47ddd50e764a2668d7fc0ce5fe8a3a2a Mon Sep 17 00:00:00 2001 From: ritul jasuja Date: Wed, 24 Jan 2018 19:18:54 +0530 Subject: [PATCH] Send RTPS header and payload in one stream This avoids assembling the header and payload on the receiver side --- .../microRTPS_client.cpp.template | 8 +++++-- .../urtps/microRTPS_agent.cpp.template | 6 +++-- msg/templates/urtps/microRTPS_transport.cpp | 23 ++++++++++--------- msg/templates/urtps/microRTPS_transport.h | 2 ++ 4 files changed, 24 insertions(+), 15 deletions(-) diff --git a/msg/templates/uorb_microcdr/microRTPS_client.cpp.template b/msg/templates/uorb_microcdr/microRTPS_client.cpp.template index 500c961066..2801a7bcee 100644 --- a/msg/templates/uorb_microcdr/microRTPS_client.cpp.template +++ b/msg/templates/uorb_microcdr/microRTPS_client.cpp.template @@ -79,6 +79,7 @@ void* send(void* /*unused*/) uint64_t sent = 0, total_sent = 0; int loop = 0, read = 0; uint32_t length = 0; + uint16_t header_length = 0; /* subscribe to topics */ int fds[@(len(send_topics))] = {}; @@ -91,7 +92,8 @@ void* send(void* /*unused*/) // microBuffer to serialized using the user defined buffer struct microBuffer microBufferWriter; - initStaticAlignedBuffer(data_buffer, BUFFER_SIZE, µBufferWriter); + header_length=transport_node->get_header_length(); + initStaticAlignedBuffer(&data_buffer[header_length], BUFFER_SIZE-header_length, µBufferWriter); // microCDR structs for managing the microBuffer struct microCDR microCDRWriter; initMicroCDR(µCDRWriter, µBufferWriter); @@ -110,7 +112,9 @@ void* send(void* /*unused*/) struct @(topic)_s data; // copy raw data into local buffer if (orb_copy(ORB_ID(@(topic)), fds[@(idx)], &data) == 0) { - serialize_@(topic)(&data, data_buffer, &length, µCDRWriter); + /* payload is shifted by header length to make room for header*/ + serialize_@(topic)(&data, &data_buffer[header_length], &length, µCDRWriter); + if (0 < (read = transport_node->write((char)@(message_id(topic)), data_buffer, length))) { total_sent += read; diff --git a/msg/templates/urtps/microRTPS_agent.cpp.template b/msg/templates/urtps/microRTPS_agent.cpp.template index 9c6e5162e9..a1936882ae 100644 --- a/msg/templates/urtps/microRTPS_agent.cpp.template +++ b/msg/templates/urtps/microRTPS_agent.cpp.template @@ -193,12 +193,14 @@ void t_send(void *data) // Send subscribed topics over UART while (topics.hasMsg(&topic_ID)) { - eprosima::fastcdr::FastBuffer cdrbuffer(data_buffer, sizeof(data_buffer)); + uint16_t header_length = get_header_length(); + /* make room for the header to fill in later */ + eprosima::fastcdr::FastBuffer cdrbuffer(&data_buffer[header_length], sizeof(data_buffer)-header_length); eprosima::fastcdr::Cdr scdr(cdrbuffer); if (topics.getMsg(topic_ID, scdr)) { length = scdr.getSerializedDataLength(); - if (0 < (length = transport_node->write(topic_ID, scdr.getBufferPointer(), length))) + if (0 < (length = transport_node->write(topic_ID, data_buffer, length))) { total_sent += length; ++sent; diff --git a/msg/templates/urtps/microRTPS_transport.cpp b/msg/templates/urtps/microRTPS_transport.cpp index 264ff1f40d..de97d7d61c 100644 --- a/msg/templates/urtps/microRTPS_transport.cpp +++ b/msg/templates/urtps/microRTPS_transport.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include "microRTPS_transport.h" @@ -193,6 +194,11 @@ ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer return len; } +ssize_t Transport_node::get_header_length() +{ + return sizeof(struct Header); +} + ssize_t Transport_node::write(const uint8_t topic_ID, char buffer[], size_t length) { if (!fds_OK()) { @@ -214,7 +220,7 @@ ssize_t Transport_node::write(const uint8_t topic_ID, char buffer[], size_t leng // [>,>,>,topic_ID,seq,payload_length,CRCHigh,CRCLow,payload_start, ... ,payload_end] - uint16_t crc = crc16((uint8_t *)buffer, length); + uint16_t crc = crc16((uint8_t *)&buffer[sizeof(header)], length); header.topic_ID = topic_ID; header.seq = seq++; @@ -223,18 +229,13 @@ ssize_t Transport_node::write(const uint8_t topic_ID, char buffer[], size_t leng header.crc_h = (crc >> 8) & 0xff; header.crc_l = crc & 0xff; - ssize_t len = node_write(&header, sizeof(header)); - - if (len != sizeof(header)) { + /* Headroom for header is created in client */ + /*Fill in the header in the same payload buffer to call a single node_write */ + memcpy(buffer, &header, sizeof(header)); + ssize_t len = node_write(buffer, length + sizeof(header)); + if (len != ssize_t(length + sizeof(header))) { goto err; } - - len = node_write(buffer, length); - - if (len != ssize_t(length)) { - goto err; - } - return len + sizeof(header); err: diff --git a/msg/templates/urtps/microRTPS_transport.h b/msg/templates/urtps/microRTPS_transport.h index 5ef27e5d28..f48dbe5283 100644 --- a/msg/templates/urtps/microRTPS_transport.h +++ b/msg/templates/urtps/microRTPS_transport.h @@ -46,6 +46,8 @@ public: virtual uint8_t close() {return 0;} ssize_t read(uint8_t *topic_ID, char out_buffer[], size_t buffer_len); ssize_t write(const uint8_t topic_ID, char buffer[], size_t length); + /* Get the Length of struct Header to make headroom for the size of struct Header alongwith payload*/ + ssize_t get_header_length(); protected: virtual ssize_t node_read(void *buffer, size_t len) = 0;