S Morita / mbed-mros2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   mbed-os-example-mros2 example-mbed-mros2-sub-pose example-mbed-mros2-pub-twist example-mbed-mros2-mturtle-teleop

embeddedRTPS/include/rtps/messages/MessageFactory.h@7:c80f65422d99, 2022-03-19 (annotated)

Committer:
smoritaemb
Date:
Sat Mar 19 09:23:37 2022 +0900
Revision:
7:c80f65422d99
Parent:
0:580aba13d1a1 
Merge test_assortment_of_msgs branch.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
smoritaemb 0:580aba13d1a1 1 /*
smoritaemb 0:580aba13d1a1 2 The MIT License
smoritaemb 0:580aba13d1a1 3 Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University
smoritaemb 0:580aba13d1a1 4 Permission is hereby granted, free of charge, to any person obtaining a copy
smoritaemb 0:580aba13d1a1 5 of this software and associated documentation files (the "Software"), to deal
smoritaemb 0:580aba13d1a1 6 in the Software without restriction, including without limitation the rights
smoritaemb 0:580aba13d1a1 7 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
smoritaemb 0:580aba13d1a1 8 copies of the Software, and to permit persons to whom the Software is
smoritaemb 0:580aba13d1a1 9 furnished to do so, subject to the following conditions:
smoritaemb 0:580aba13d1a1 10 The above copyright notice and this permission notice shall be included in
smoritaemb 0:580aba13d1a1 11 all copies or substantial portions of the Software.
smoritaemb 0:580aba13d1a1 12 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
smoritaemb 0:580aba13d1a1 13 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
smoritaemb 0:580aba13d1a1 14 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
smoritaemb 0:580aba13d1a1 15 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
smoritaemb 0:580aba13d1a1 16 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
smoritaemb 0:580aba13d1a1 17 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
smoritaemb 0:580aba13d1a1 18 THE SOFTWARE
smoritaemb 0:580aba13d1a1 19
smoritaemb 0:580aba13d1a1 20 This file is part of embeddedRTPS.
smoritaemb 0:580aba13d1a1 21
smoritaemb 0:580aba13d1a1 22 Author: i11 - Embedded Software, RWTH Aachen University
smoritaemb 0:580aba13d1a1 23 */
smoritaemb 0:580aba13d1a1 24
smoritaemb 0:580aba13d1a1 25 #ifndef RTPS_MESSAGEFACTORY_H
smoritaemb 0:580aba13d1a1 26 #define RTPS_MESSAGEFACTORY_H
smoritaemb 0:580aba13d1a1 27
smoritaemb 0:580aba13d1a1 28 #include "rtps/common/types.h"
smoritaemb 0:580aba13d1a1 29 #include "rtps/config.h"
smoritaemb 0:580aba13d1a1 30 #include "rtps/messages/MessageTypes.h"
smoritaemb 0:580aba13d1a1 31 #include "rtps/utils/sysFunctions.h"
smoritaemb 0:580aba13d1a1 32
smoritaemb 0:580aba13d1a1 33 #include <array>
smoritaemb 0:580aba13d1a1 34 #include <cstdint>
smoritaemb 0:580aba13d1a1 35
smoritaemb 0:580aba13d1a1 36 namespace rtps {
smoritaemb 0:580aba13d1a1 37 namespace MessageFactory {
smoritaemb 0:580aba13d1a1 38 const std::array<uint8_t, 4> PROTOCOL_TYPE{'R', 'T', 'P', 'S'};
smoritaemb 0:580aba13d1a1 39 const uint8_t numBytesUntilEndOfLength =
smoritaemb 0:580aba13d1a1 40 4; // The first bytes incl. submessagelength don't count
smoritaemb 0:580aba13d1a1 41
smoritaemb 0:580aba13d1a1 42 template <class Buffer>
smoritaemb 0:580aba13d1a1 43 void addHeader(Buffer &buffer, const GuidPrefix_t &guidPrefix) {
smoritaemb 0:580aba13d1a1 44
smoritaemb 0:580aba13d1a1 45 Header header;
smoritaemb 0:580aba13d1a1 46 header.protocolName = PROTOCOL_TYPE;
smoritaemb 0:580aba13d1a1 47 header.protocolVersion = PROTOCOLVERSION;
smoritaemb 0:580aba13d1a1 48 header.vendorId = Config::VENDOR_ID;
smoritaemb 0:580aba13d1a1 49 header.guidPrefix = guidPrefix;
smoritaemb 0:580aba13d1a1 50
smoritaemb 0:580aba13d1a1 51 serializeMessage(buffer, header);
smoritaemb 0:580aba13d1a1 52 }
smoritaemb 0:580aba13d1a1 53
smoritaemb 0:580aba13d1a1 54 template <class Buffer>
smoritaemb 0:580aba13d1a1 55 void addSubMessageTimeStamp(Buffer &buffer, bool setInvalid = false) {
smoritaemb 0:580aba13d1a1 56 SubmessageHeader header;
smoritaemb 0:580aba13d1a1 57 header.submessageId = SubmessageKind::INFO_TS;
smoritaemb 0:580aba13d1a1 58
smoritaemb 0:580aba13d1a1 59 #if IS_LITTLE_ENDIAN
smoritaemb 0:580aba13d1a1 60 header.flags = FLAG_LITTLE_ENDIAN;
smoritaemb 0:580aba13d1a1 61 #else
smoritaemb 0:580aba13d1a1 62 header.flags = FLAG_BIG_ENDIAN;
smoritaemb 0:580aba13d1a1 63 #endif
smoritaemb 0:580aba13d1a1 64
smoritaemb 0:580aba13d1a1 65 if (setInvalid) {
smoritaemb 0:580aba13d1a1 66 header.flags |= FLAG_INVALIDATE;
smoritaemb 0:580aba13d1a1 67 header.submessageLength = 0;
smoritaemb 0:580aba13d1a1 68 } else {
smoritaemb 0:580aba13d1a1 69 header.submessageLength = sizeof(Time_t);
smoritaemb 0:580aba13d1a1 70 }
smoritaemb 0:580aba13d1a1 71
smoritaemb 0:580aba13d1a1 72 serializeMessage(buffer, header);
smoritaemb 0:580aba13d1a1 73
smoritaemb 0:580aba13d1a1 74 if (!setInvalid) {
smoritaemb 0:580aba13d1a1 75 buffer.reserve(header.submessageLength);
smoritaemb 0:580aba13d1a1 76 Time_t now = getCurrentTimeStamp();
smoritaemb 0:580aba13d1a1 77 // buffer.append(reinterpret_cast<uint8_t *>(&now.seconds),
smoritaemb 0:580aba13d1a1 78 // sizeof(Time_t::seconds));
smoritaemb 0:580aba13d1a1 79 // buffer.append(reinterpret_cast<uint8_t *>(&now.fraction),
smoritaemb 0:580aba13d1a1 80 // sizeof(Time_t::fraction));
smoritaemb 0:580aba13d1a1 81 uint8_t time_arr[8] = {0xff, 0x3f, 0x04, 0x60, 0x00, 0xd4, 0x92, 0xa6};
smoritaemb 0:580aba13d1a1 82 buffer.append(time_arr, 8);
smoritaemb 0:580aba13d1a1 83 }
smoritaemb 0:580aba13d1a1 84 }
smoritaemb 0:580aba13d1a1 85
smoritaemb 0:580aba13d1a1 86 template <class Buffer>
smoritaemb 0:580aba13d1a1 87 void addSubMessageDestination(Buffer &buffer, std::array<unsigned char, 12>::pointer id_ptr) {
smoritaemb 0:580aba13d1a1 88 buffer.reserve(16);
smoritaemb 0:580aba13d1a1 89 uint8_t info_dst[16] = {0x0e, 0x01, 0x0c, 0x00, 0x01, 0x0f, 0xf2, 0x05, 0xa8, 0x03, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00};
smoritaemb 0:580aba13d1a1 90 memcpy(&info_dst[4], id_ptr, 12);
smoritaemb 0:580aba13d1a1 91 buffer.append(info_dst, 16);
smoritaemb 0:580aba13d1a1 92 }
smoritaemb 0:580aba13d1a1 93
smoritaemb 0:580aba13d1a1 94 template <class Buffer>
smoritaemb 0:580aba13d1a1 95 void addSubMessageDestination(Buffer &buffer) {
smoritaemb 0:580aba13d1a1 96 buffer.reserve(16);
smoritaemb 0:580aba13d1a1 97 uint8_t info_dst[16] = {0x0e, 0x01, 0x0c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
smoritaemb 0:580aba13d1a1 98 buffer.append(info_dst, 16);
smoritaemb 0:580aba13d1a1 99 }
smoritaemb 0:580aba13d1a1 100
smoritaemb 0:580aba13d1a1 101 template <class Buffer>
smoritaemb 0:580aba13d1a1 102 void addSubMessageData(Buffer &buffer, const Buffer &filledPayload,
smoritaemb 0:580aba13d1a1 103 bool containsInlineQos, const SequenceNumber_t &SN,
smoritaemb 0:580aba13d1a1 104 const EntityId_t &writerID, const EntityId_t &readerID) {
smoritaemb 0:580aba13d1a1 105 SubmessageData msg;
smoritaemb 0:580aba13d1a1 106 msg.header.submessageId = SubmessageKind::DATA;
smoritaemb 0:580aba13d1a1 107 #if IS_LITTLE_ENDIAN
smoritaemb 0:580aba13d1a1 108 msg.header.flags = FLAG_LITTLE_ENDIAN;
smoritaemb 0:580aba13d1a1 109 #else
smoritaemb 0:580aba13d1a1 110 msg.header.flags = FLAG_BIG_ENDIAN;
smoritaemb 0:580aba13d1a1 111 #endif
smoritaemb 0:580aba13d1a1 112
smoritaemb 0:580aba13d1a1 113 msg.header.submessageLength = SubmessageData::getRawSize() +
smoritaemb 0:580aba13d1a1 114 filledPayload.spaceUsed() -
smoritaemb 0:580aba13d1a1 115 numBytesUntilEndOfLength;
smoritaemb 0:580aba13d1a1 116
smoritaemb 0:580aba13d1a1 117 if (containsInlineQos) {
smoritaemb 0:580aba13d1a1 118 msg.header.flags |= FLAG_INLINE_QOS;
smoritaemb 0:580aba13d1a1 119 }
smoritaemb 0:580aba13d1a1 120 if (filledPayload.isValid()) {
smoritaemb 0:580aba13d1a1 121 msg.header.flags |= FLAG_DATA_PAYLOAD;
smoritaemb 0:580aba13d1a1 122 }
smoritaemb 0:580aba13d1a1 123
smoritaemb 0:580aba13d1a1 124 msg.writerSN = SN;
smoritaemb 0:580aba13d1a1 125 msg.extraFlags = 0;
smoritaemb 0:580aba13d1a1 126 msg.readerId = readerID;
smoritaemb 0:580aba13d1a1 127 msg.writerId = writerID;
smoritaemb 0:580aba13d1a1 128
smoritaemb 0:580aba13d1a1 129 constexpr uint16_t octetsToInlineQoS =
smoritaemb 0:580aba13d1a1 130 4 + 4 + 8; // EntityIds + SequenceNumber
smoritaemb 0:580aba13d1a1 131 msg.octetsToInlineQos = octetsToInlineQoS;
smoritaemb 0:580aba13d1a1 132
smoritaemb 0:580aba13d1a1 133 serializeMessage(buffer, msg);
smoritaemb 0:580aba13d1a1 134
smoritaemb 0:580aba13d1a1 135 if (filledPayload.isValid()) {
smoritaemb 0:580aba13d1a1 136 Buffer shallowCopy = filledPayload;
smoritaemb 0:580aba13d1a1 137 buffer.append(std::move(shallowCopy));
smoritaemb 0:580aba13d1a1 138 }
smoritaemb 0:580aba13d1a1 139 }
smoritaemb 0:580aba13d1a1 140
smoritaemb 0:580aba13d1a1 141 template <class Buffer>
smoritaemb 0:580aba13d1a1 142 void addHeartbeat(Buffer &buffer, EntityId_t writerId, EntityId_t readerId,
smoritaemb 0:580aba13d1a1 143 SequenceNumber_t firstSN, SequenceNumber_t lastSN,
smoritaemb 0:580aba13d1a1 144 Count_t count) {
smoritaemb 0:580aba13d1a1 145 SubmessageHeartbeat subMsg;
smoritaemb 0:580aba13d1a1 146 subMsg.header.submessageId = SubmessageKind::HEARTBEAT;
smoritaemb 0:580aba13d1a1 147 subMsg.header.submessageLength =
smoritaemb 0:580aba13d1a1 148 SubmessageHeartbeat::getRawSize() - numBytesUntilEndOfLength;
smoritaemb 0:580aba13d1a1 149 #if IS_LITTLE_ENDIAN
smoritaemb 0:580aba13d1a1 150 subMsg.header.flags = FLAG_LITTLE_ENDIAN;
smoritaemb 0:580aba13d1a1 151 #else
smoritaemb 0:580aba13d1a1 152 subMsg.header.flags = FLAG_BIG_ENDIAN;
smoritaemb 0:580aba13d1a1 153 #endif
smoritaemb 0:580aba13d1a1 154 // Force response by not setting final flag.
smoritaemb 0:580aba13d1a1 155
smoritaemb 0:580aba13d1a1 156 subMsg.writerId = writerId;
smoritaemb 0:580aba13d1a1 157 subMsg.readerId = readerId;
smoritaemb 0:580aba13d1a1 158 subMsg.firstSN = firstSN;
smoritaemb 0:580aba13d1a1 159 subMsg.lastSN = lastSN;
smoritaemb 0:580aba13d1a1 160 subMsg.count = count;
smoritaemb 0:580aba13d1a1 161
smoritaemb 0:580aba13d1a1 162 serializeMessage(buffer, subMsg);
smoritaemb 0:580aba13d1a1 163 }
smoritaemb 0:580aba13d1a1 164
smoritaemb 0:580aba13d1a1 165 template <class Buffer>
smoritaemb 0:580aba13d1a1 166 void addAckNack(Buffer &buffer, EntityId_t writerId, EntityId_t readerId,
smoritaemb 0:580aba13d1a1 167 SequenceNumberSet readerSNState, Count_t count) {
smoritaemb 0:580aba13d1a1 168 SubmessageAckNack subMsg;
smoritaemb 0:580aba13d1a1 169 subMsg.header.submessageId = SubmessageKind::ACKNACK;
smoritaemb 0:580aba13d1a1 170 #if IS_LITTLE_ENDIAN
smoritaemb 0:580aba13d1a1 171 subMsg.header.flags = FLAG_LITTLE_ENDIAN;
smoritaemb 0:580aba13d1a1 172 #else
smoritaemb 0:580aba13d1a1 173 subMsg.header.flags = FLAG_BIG_ENDIAN;
smoritaemb 0:580aba13d1a1 174 #endif
smoritaemb 0:580aba13d1a1 175 subMsg.header.flags |= FLAG_FINAL; // For now, we don't want any response
smoritaemb 0:580aba13d1a1 176 subMsg.header.submessageLength =
smoritaemb 0:580aba13d1a1 177 SubmessageAckNack::getRawSize(readerSNState) - numBytesUntilEndOfLength;
smoritaemb 0:580aba13d1a1 178
smoritaemb 0:580aba13d1a1 179 subMsg.writerId = writerId;
smoritaemb 0:580aba13d1a1 180 subMsg.readerId = readerId;
smoritaemb 0:580aba13d1a1 181 subMsg.readerSNState = readerSNState;
smoritaemb 0:580aba13d1a1 182 subMsg.count = count;
smoritaemb 0:580aba13d1a1 183
smoritaemb 0:580aba13d1a1 184 serializeMessage(buffer, subMsg);
smoritaemb 0:580aba13d1a1 185 }
smoritaemb 0:580aba13d1a1 186 } // namespace MessageFactory
smoritaemb 0:580aba13d1a1 187 } // namespace rtps
smoritaemb 0:580aba13d1a1 188
smoritaemb 0:580aba13d1a1 189 #endif // RTPS_MESSAGEFACTORY_H