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/src/discovery/SPDPAgent.cpp@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 #include "rtps/discovery/SPDPAgent.h"
smoritaemb 0:580aba13d1a1 26 #include "lwip/sys.h"
smoritaemb 0:580aba13d1a1 27 #include "rtps/discovery/ParticipantProxyData.h"
smoritaemb 0:580aba13d1a1 28 #include "rtps/entities/Participant.h"
smoritaemb 0:580aba13d1a1 29 #include "rtps/entities/Reader.h"
smoritaemb 0:580aba13d1a1 30 #include "rtps/entities/Writer.h"
smoritaemb 0:580aba13d1a1 31 #include "rtps/messages/MessageTypes.h"
smoritaemb 0:580aba13d1a1 32 #include "rtps/utils/udpUtils.h"
smoritaemb 0:580aba13d1a1 33
smoritaemb 0:580aba13d1a1 34 using rtps::SPDPAgent;
smoritaemb 0:580aba13d1a1 35 using rtps::SMElement::BuildInEndpointSet;
smoritaemb 0:580aba13d1a1 36 using rtps::SMElement::ParameterId;
smoritaemb 0:580aba13d1a1 37
smoritaemb 0:580aba13d1a1 38 #define SPDP_VERBOSE 0
smoritaemb 0:580aba13d1a1 39
smoritaemb 0:580aba13d1a1 40 #if SPDP_VERBOSE
smoritaemb 0:580aba13d1a1 41 #include "rtps/utils/printutils.h"
smoritaemb 0:580aba13d1a1 42 #endif
smoritaemb 0:580aba13d1a1 43
smoritaemb 0:580aba13d1a1 44 SPDPAgent::~SPDPAgent() {
smoritaemb 0:580aba13d1a1 45 if (initialized) {
smoritaemb 0:580aba13d1a1 46 sys_mutex_free(&m_mutex);
smoritaemb 0:580aba13d1a1 47 }
smoritaemb 0:580aba13d1a1 48 }
smoritaemb 0:580aba13d1a1 49
smoritaemb 0:580aba13d1a1 50 void SPDPAgent::init(Participant &participant, BuiltInEndpoints &endpoints) {
smoritaemb 0:580aba13d1a1 51 if (sys_mutex_new(&m_mutex) != ERR_OK) {
smoritaemb 0:580aba13d1a1 52 #if SPDP_VERBOSE
smoritaemb 0:580aba13d1a1 53 printf("Could not alloc mutex");
smoritaemb 0:580aba13d1a1 54 #endif
smoritaemb 0:580aba13d1a1 55 return;
smoritaemb 0:580aba13d1a1 56 }
smoritaemb 0:580aba13d1a1 57 mp_participant = &participant;
smoritaemb 0:580aba13d1a1 58 m_buildInEndpoints = endpoints;
smoritaemb 0:580aba13d1a1 59 m_buildInEndpoints.spdpReader->registerCallback(receiveCallback, this);
smoritaemb 0:580aba13d1a1 60
smoritaemb 0:580aba13d1a1 61 ucdr_init_buffer(&m_microbuffer, m_outputBuffer.data(),
smoritaemb 0:580aba13d1a1 62 m_outputBuffer.size());
smoritaemb 0:580aba13d1a1 63 // addInlineQos();
smoritaemb 0:580aba13d1a1 64 addParticipantParameters();
smoritaemb 0:580aba13d1a1 65 initialized = true;
smoritaemb 0:580aba13d1a1 66 }
smoritaemb 0:580aba13d1a1 67
smoritaemb 0:580aba13d1a1 68 void SPDPAgent::start() {
smoritaemb 0:580aba13d1a1 69 if (m_running) {
smoritaemb 0:580aba13d1a1 70 return;
smoritaemb 0:580aba13d1a1 71 }
smoritaemb 0:580aba13d1a1 72 m_running = true;
smoritaemb 0:580aba13d1a1 73 #ifdef MROS2_USE_EMBEDDEDRTPS
smoritaemb 0:580aba13d1a1 74 sys_thread_new("SPDPThread", callRunBroadcast, this,
smoritaemb 0:580aba13d1a1 75 Config::SPDP_WRITER_STACKSIZE, Config::SPDP_WRITER_PRIO);
smoritaemb 0:580aba13d1a1 76 #else
smoritaemb 0:580aba13d1a1 77 sys_thread_new("SPDPThread", runBroadcast, this,
smoritaemb 0:580aba13d1a1 78 Config::SPDP_WRITER_STACKSIZE, Config::SPDP_WRITER_PRIO);
smoritaemb 0:580aba13d1a1 79 #endif
smoritaemb 0:580aba13d1a1 80 }
smoritaemb 0:580aba13d1a1 81
smoritaemb 0:580aba13d1a1 82 void SPDPAgent::stop() { m_running = false; }
smoritaemb 0:580aba13d1a1 83
smoritaemb 0:580aba13d1a1 84 void SPDPAgent::runBroadcast(void *args) {
smoritaemb 0:580aba13d1a1 85 SPDPAgent &agent = *static_cast<SPDPAgent *>(args);
smoritaemb 0:580aba13d1a1 86 const DataSize_t size = ucdr_buffer_length(&agent.m_microbuffer);
smoritaemb 0:580aba13d1a1 87 agent.m_buildInEndpoints.spdpWriter->newChange(
smoritaemb 0:580aba13d1a1 88 ChangeKind_t::ALIVE, agent.m_microbuffer.init, size);
smoritaemb 0:580aba13d1a1 89 while (agent.m_running) {
smoritaemb 0:580aba13d1a1 90 sys_msleep(Config::SPDP_RESEND_PERIOD_MS);
smoritaemb 0:580aba13d1a1 91 agent.m_buildInEndpoints.spdpWriter->setAllChangesToUnsent();
smoritaemb 0:580aba13d1a1 92 }
smoritaemb 0:580aba13d1a1 93 }
smoritaemb 0:580aba13d1a1 94
smoritaemb 0:580aba13d1a1 95 void SPDPAgent::receiveCallback(void *callee,
smoritaemb 0:580aba13d1a1 96 const ReaderCacheChange &cacheChange) {
smoritaemb 0:580aba13d1a1 97 auto agent = static_cast<SPDPAgent *>(callee);
smoritaemb 0:580aba13d1a1 98 agent->handleSPDPPackage(cacheChange);
smoritaemb 0:580aba13d1a1 99 }
smoritaemb 0:580aba13d1a1 100
smoritaemb 0:580aba13d1a1 101 void SPDPAgent::handleSPDPPackage(const ReaderCacheChange &cacheChange) {
smoritaemb 0:580aba13d1a1 102 if (!initialized) {
smoritaemb 0:580aba13d1a1 103 #if SPDP_VERBOSE
smoritaemb 0:580aba13d1a1 104 printf("SPDP: Callback called without initialization\n");
smoritaemb 0:580aba13d1a1 105 #endif
smoritaemb 0:580aba13d1a1 106 return;
smoritaemb 0:580aba13d1a1 107 }
smoritaemb 0:580aba13d1a1 108
smoritaemb 0:580aba13d1a1 109 Lock lock{m_mutex};
smoritaemb 0:580aba13d1a1 110 if (cacheChange.size > m_inputBuffer.size()) {
smoritaemb 0:580aba13d1a1 111 #if SPDP_VERBOSE
smoritaemb 0:580aba13d1a1 112 printf("SPDP: Input buffer to small\n");
smoritaemb 0:580aba13d1a1 113 #endif
smoritaemb 0:580aba13d1a1 114 return;
smoritaemb 0:580aba13d1a1 115 }
smoritaemb 0:580aba13d1a1 116
smoritaemb 0:580aba13d1a1 117 // Something went wrong deserializing remote participant
smoritaemb 0:580aba13d1a1 118 if (!cacheChange.copyInto(m_inputBuffer.data(), m_inputBuffer.size())) {
smoritaemb 0:580aba13d1a1 119 return;
smoritaemb 0:580aba13d1a1 120 }
smoritaemb 0:580aba13d1a1 121
smoritaemb 0:580aba13d1a1 122 ucdrBuffer buffer;
smoritaemb 0:580aba13d1a1 123 ucdr_init_buffer(&buffer, m_inputBuffer.data(), m_inputBuffer.size());
smoritaemb 0:580aba13d1a1 124
smoritaemb 0:580aba13d1a1 125 if (cacheChange.kind == ChangeKind_t::ALIVE) {
smoritaemb 0:580aba13d1a1 126 configureEndianessAndOptions(buffer);
smoritaemb 0:580aba13d1a1 127 volatile bool success = m_proxyDataBuffer.readFromUcdrBuffer(buffer);
smoritaemb 0:580aba13d1a1 128 if (success) {
smoritaemb 0:580aba13d1a1 129 // TODO In case we store the history we can free the history mutex here
smoritaemb 0:580aba13d1a1 130 processProxyData();
smoritaemb 0:580aba13d1a1 131 }
smoritaemb 0:580aba13d1a1 132 } else {
smoritaemb 0:580aba13d1a1 133 // TODO RemoveParticipant
smoritaemb 0:580aba13d1a1 134 }
smoritaemb 0:580aba13d1a1 135 }
smoritaemb 0:580aba13d1a1 136
smoritaemb 0:580aba13d1a1 137 void SPDPAgent::configureEndianessAndOptions(ucdrBuffer &buffer) {
smoritaemb 0:580aba13d1a1 138 std::array<uint8_t, 2> encapsulation{};
smoritaemb 0:580aba13d1a1 139 // Endianess doesn't matter for this since those are single bytes
smoritaemb 0:580aba13d1a1 140 ucdr_deserialize_array_uint8_t(&buffer, encapsulation.data(),
smoritaemb 0:580aba13d1a1 141 encapsulation.size());
smoritaemb 0:580aba13d1a1 142 if (encapsulation == SMElement::SCHEME_PL_CDR_LE) {
smoritaemb 0:580aba13d1a1 143 buffer.endianness = UCDR_LITTLE_ENDIANNESS;
smoritaemb 0:580aba13d1a1 144 } else {
smoritaemb 0:580aba13d1a1 145 buffer.endianness = UCDR_BIG_ENDIANNESS;
smoritaemb 0:580aba13d1a1 146 }
smoritaemb 0:580aba13d1a1 147 // Reuse encapsulation buffer to skip options
smoritaemb 0:580aba13d1a1 148 ucdr_deserialize_array_uint8_t(&buffer, encapsulation.data(),
smoritaemb 0:580aba13d1a1 149 encapsulation.size());
smoritaemb 0:580aba13d1a1 150 }
smoritaemb 0:580aba13d1a1 151
smoritaemb 0:580aba13d1a1 152 void SPDPAgent::processProxyData() {
smoritaemb 0:580aba13d1a1 153 if (m_proxyDataBuffer.m_guid.prefix.id == mp_participant->m_guidPrefix.id) {
smoritaemb 0:580aba13d1a1 154 return; // Our own packet
smoritaemb 0:580aba13d1a1 155 }
smoritaemb 0:580aba13d1a1 156
smoritaemb 0:580aba13d1a1 157 if (mp_participant->findRemoteParticipant(m_proxyDataBuffer.m_guid.prefix) !=
smoritaemb 0:580aba13d1a1 158 nullptr) {
smoritaemb 0:580aba13d1a1 159 m_buildInEndpoints.spdpWriter->setAllChangesToUnsent();
smoritaemb 0:580aba13d1a1 160 return; // Already in our list
smoritaemb 0:580aba13d1a1 161 }
smoritaemb 0:580aba13d1a1 162
smoritaemb 0:580aba13d1a1 163 // New participant, help him join fast by broadcasting data again
smoritaemb 0:580aba13d1a1 164 // if(mp_participant->getRemoteParticipantCount() == 1){
smoritaemb 0:580aba13d1a1 165 // return;
smoritaemb 0:580aba13d1a1 166 //}
smoritaemb 0:580aba13d1a1 167
smoritaemb 0:580aba13d1a1 168 if (mp_participant->addNewRemoteParticipant(m_proxyDataBuffer)) {
smoritaemb 0:580aba13d1a1 169 addProxiesForBuiltInEndpoints();
smoritaemb 0:580aba13d1a1 170 m_buildInEndpoints.spdpWriter->setAllChangesToUnsent();
smoritaemb 0:580aba13d1a1 171 #if SPDP_VERBOSE
smoritaemb 0:580aba13d1a1 172 printf("Added new participant with guid: ");
smoritaemb 0:580aba13d1a1 173 printGuidPrefix(m_proxyDataBuffer.m_guid.prefix);
smoritaemb 0:580aba13d1a1 174 printf("\n");
smoritaemb 0:580aba13d1a1 175 } else {
smoritaemb 0:580aba13d1a1 176 printf("Failed to add new participant");
smoritaemb 0:580aba13d1a1 177 #endif
smoritaemb 0:580aba13d1a1 178 }
smoritaemb 0:580aba13d1a1 179 }
smoritaemb 0:580aba13d1a1 180
smoritaemb 0:580aba13d1a1 181 bool SPDPAgent::addProxiesForBuiltInEndpoints() {
smoritaemb 0:580aba13d1a1 182
smoritaemb 0:580aba13d1a1 183 Locator *locator = nullptr;
smoritaemb 0:580aba13d1a1 184
smoritaemb 0:580aba13d1a1 185 // Check if the remote participants has a locator in our subnet
smoritaemb 0:580aba13d1a1 186 for (unsigned int i = 0;
smoritaemb 0:580aba13d1a1 187 i < m_proxyDataBuffer.m_metatrafficUnicastLocatorList.size(); i++) {
smoritaemb 0:580aba13d1a1 188 Locator *l = &(m_proxyDataBuffer.m_metatrafficUnicastLocatorList[i]);
smoritaemb 0:580aba13d1a1 189 if (l->isValid() && l->isSameSubnet()) {
smoritaemb 0:580aba13d1a1 190 locator = l;
smoritaemb 0:580aba13d1a1 191 break;
smoritaemb 0:580aba13d1a1 192 }
smoritaemb 0:580aba13d1a1 193 }
smoritaemb 0:580aba13d1a1 194
smoritaemb 0:580aba13d1a1 195 if (!locator) {
smoritaemb 0:580aba13d1a1 196 return false;
smoritaemb 0:580aba13d1a1 197 }
smoritaemb 0:580aba13d1a1 198
smoritaemb 0:580aba13d1a1 199 ip4_addr_t ip4addr = locator->getIp4Address();
smoritaemb 0:580aba13d1a1 200 const char *addr = ip4addr_ntoa(&ip4addr);
smoritaemb 0:580aba13d1a1 201 printf("Adding IPv4 Locator %s", addr);
smoritaemb 0:580aba13d1a1 202
smoritaemb 0:580aba13d1a1 203 if (m_proxyDataBuffer.hasPublicationWriter()) {
smoritaemb 0:580aba13d1a1 204 const WriterProxy proxy{{m_proxyDataBuffer.m_guid.prefix,
smoritaemb 0:580aba13d1a1 205 ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER},
smoritaemb 0:580aba13d1a1 206 *locator};
smoritaemb 0:580aba13d1a1 207 m_buildInEndpoints.sedpPubReader->addNewMatchedWriter(proxy);
smoritaemb 0:580aba13d1a1 208 }
smoritaemb 0:580aba13d1a1 209
smoritaemb 0:580aba13d1a1 210 if (m_proxyDataBuffer.hasSubscriptionWriter()) {
smoritaemb 0:580aba13d1a1 211 const WriterProxy proxy{{m_proxyDataBuffer.m_guid.prefix,
smoritaemb 0:580aba13d1a1 212 ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER},
smoritaemb 0:580aba13d1a1 213 *locator};
smoritaemb 0:580aba13d1a1 214 m_buildInEndpoints.sedpSubReader->addNewMatchedWriter(proxy);
smoritaemb 0:580aba13d1a1 215 }
smoritaemb 0:580aba13d1a1 216
smoritaemb 0:580aba13d1a1 217 if (m_proxyDataBuffer.hasPublicationReader()) {
smoritaemb 0:580aba13d1a1 218 const ReaderProxy proxy{{m_proxyDataBuffer.m_guid.prefix,
smoritaemb 0:580aba13d1a1 219 ENTITYID_SEDP_BUILTIN_PUBLICATIONS_READER},
smoritaemb 0:580aba13d1a1 220 *locator};
smoritaemb 0:580aba13d1a1 221 m_buildInEndpoints.sedpPubWriter->addNewMatchedReader(proxy);
smoritaemb 0:580aba13d1a1 222 }
smoritaemb 0:580aba13d1a1 223
smoritaemb 0:580aba13d1a1 224 if (m_proxyDataBuffer.hasSubscriptionReader()) {
smoritaemb 0:580aba13d1a1 225 const ReaderProxy proxy{{m_proxyDataBuffer.m_guid.prefix,
smoritaemb 0:580aba13d1a1 226 ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_READER},
smoritaemb 0:580aba13d1a1 227 *locator};
smoritaemb 0:580aba13d1a1 228 m_buildInEndpoints.sedpSubWriter->addNewMatchedReader(proxy);
smoritaemb 0:580aba13d1a1 229 }
smoritaemb 0:580aba13d1a1 230
smoritaemb 0:580aba13d1a1 231 return true;
smoritaemb 0:580aba13d1a1 232 }
smoritaemb 0:580aba13d1a1 233
smoritaemb 0:580aba13d1a1 234 void SPDPAgent::addInlineQos() {
smoritaemb 0:580aba13d1a1 235 ucdr_serialize_uint16_t(&m_microbuffer, ParameterId::PID_KEY_HASH);
smoritaemb 0:580aba13d1a1 236 ucdr_serialize_uint16_t(&m_microbuffer, 16);
smoritaemb 0:580aba13d1a1 237 ucdr_serialize_array_uint8_t(&m_microbuffer,
smoritaemb 0:580aba13d1a1 238 mp_participant->m_guidPrefix.id.data(),
smoritaemb 0:580aba13d1a1 239 sizeof(GuidPrefix_t::id));
smoritaemb 0:580aba13d1a1 240 ucdr_serialize_array_uint8_t(&m_microbuffer,
smoritaemb 0:580aba13d1a1 241 ENTITYID_BUILD_IN_PARTICIPANT.entityKey.data(),
smoritaemb 0:580aba13d1a1 242 sizeof(EntityId_t::entityKey));
smoritaemb 0:580aba13d1a1 243 ucdr_serialize_uint8_t(
smoritaemb 0:580aba13d1a1 244 &m_microbuffer,
smoritaemb 0:580aba13d1a1 245 static_cast<uint8_t>(ENTITYID_BUILD_IN_PARTICIPANT.entityKind));
smoritaemb 0:580aba13d1a1 246
smoritaemb 0:580aba13d1a1 247 endCurrentList();
smoritaemb 0:580aba13d1a1 248 }
smoritaemb 0:580aba13d1a1 249
smoritaemb 0:580aba13d1a1 250 void SPDPAgent::endCurrentList() {
smoritaemb 0:580aba13d1a1 251 //adding node name and node namespace
smoritaemb 0:580aba13d1a1 252 /*
smoritaemb 0:580aba13d1a1 253 ucdr_serialize_uint16_t(&m_microbuffer, ParameterId::PID_ENTITY_NAME); //TODO: clean this
smoritaemb 0:580aba13d1a1 254 ucdr_serialize_uint16_t(&m_microbuffer, 8);
smoritaemb 0:580aba13d1a1 255 ucdr_serialize_uint16_t(&m_microbuffer, 4);
smoritaemb 0:580aba13d1a1 256 ucdr_serialize_uint16_t(&m_microbuffer, 0);
smoritaemb 0:580aba13d1a1 257 ucdr_serialize_uint8_t(&m_microbuffer, 's');
smoritaemb 0:580aba13d1a1 258 ucdr_serialize_uint8_t(&m_microbuffer, 't');
smoritaemb 0:580aba13d1a1 259 ucdr_serialize_uint8_t(&m_microbuffer, 'm');
smoritaemb 0:580aba13d1a1 260 ucdr_serialize_uint8_t(&m_microbuffer, 0);
smoritaemb 0:580aba13d1a1 261
smoritaemb 0:580aba13d1a1 262 ucdr_serialize_uint16_t(&m_microbuffer, ParameterId::PID_USER_DATA); //TODO: clean this
smoritaemb 0:580aba13d1a1 263 ucdr_serialize_uint16_t(&m_microbuffer, 28);
smoritaemb 0:580aba13d1a1 264 ucdr_serialize_uint16_t(&m_microbuffer, 22);
smoritaemb 0:580aba13d1a1 265 ucdr_serialize_uint16_t(&m_microbuffer, 0);
smoritaemb 0:580aba13d1a1 266 ucdr_serialize_uint8_t(&m_microbuffer, 'n');
smoritaemb 0:580aba13d1a1 267 ucdr_serialize_uint8_t(&m_microbuffer, 'a');
smoritaemb 0:580aba13d1a1 268 ucdr_serialize_uint8_t(&m_microbuffer, 'm');
smoritaemb 0:580aba13d1a1 269 ucdr_serialize_uint8_t(&m_microbuffer, 'e');
smoritaemb 0:580aba13d1a1 270 ucdr_serialize_uint8_t(&m_microbuffer, '=');
smoritaemb 0:580aba13d1a1 271 ucdr_serialize_uint8_t(&m_microbuffer, 's');
smoritaemb 0:580aba13d1a1 272 ucdr_serialize_uint8_t(&m_microbuffer, 't');
smoritaemb 0:580aba13d1a1 273 ucdr_serialize_uint8_t(&m_microbuffer, 'm');
smoritaemb 0:580aba13d1a1 274 ucdr_serialize_uint8_t(&m_microbuffer, ';');
smoritaemb 0:580aba13d1a1 275 ucdr_serialize_uint8_t(&m_microbuffer, 'n');
smoritaemb 0:580aba13d1a1 276 ucdr_serialize_uint8_t(&m_microbuffer, 'a');
smoritaemb 0:580aba13d1a1 277 ucdr_serialize_uint8_t(&m_microbuffer, 'm');
smoritaemb 0:580aba13d1a1 278 ucdr_serialize_uint8_t(&m_microbuffer, 'e');
smoritaemb 0:580aba13d1a1 279 ucdr_serialize_uint8_t(&m_microbuffer, 's');
smoritaemb 0:580aba13d1a1 280 ucdr_serialize_uint8_t(&m_microbuffer, 'p');
smoritaemb 0:580aba13d1a1 281 ucdr_serialize_uint8_t(&m_microbuffer, 'a');
smoritaemb 0:580aba13d1a1 282 ucdr_serialize_uint8_t(&m_microbuffer, 'c');
smoritaemb 0:580aba13d1a1 283 ucdr_serialize_uint8_t(&m_microbuffer, 'e');
smoritaemb 0:580aba13d1a1 284 ucdr_serialize_uint8_t(&m_microbuffer, '=');
smoritaemb 0:580aba13d1a1 285 ucdr_serialize_uint8_t(&m_microbuffer, '/');
smoritaemb 0:580aba13d1a1 286 ucdr_serialize_uint8_t(&m_microbuffer, ';');
smoritaemb 0:580aba13d1a1 287 ucdr_serialize_uint8_t(&m_microbuffer, 0);
smoritaemb 0:580aba13d1a1 288 ucdr_serialize_uint16_t(&m_microbuffer, 0);
smoritaemb 0:580aba13d1a1 289 */
smoritaemb 0:580aba13d1a1 290 ucdr_serialize_uint16_t(&m_microbuffer, ParameterId::PID_SENTINEL);
smoritaemb 0:580aba13d1a1 291 ucdr_serialize_uint16_t(&m_microbuffer, 0);
smoritaemb 0:580aba13d1a1 292 }
smoritaemb 0:580aba13d1a1 293
smoritaemb 0:580aba13d1a1 294 void SPDPAgent::addParticipantParameters() {
smoritaemb 0:580aba13d1a1 295 const uint16_t zero_options = 0;
smoritaemb 0:580aba13d1a1 296 const uint16_t protocolVersionSize =
smoritaemb 0:580aba13d1a1 297 sizeof(PROTOCOLVERSION.major) + sizeof(PROTOCOLVERSION.minor);
smoritaemb 0:580aba13d1a1 298 const uint16_t vendorIdSize = Config::VENDOR_ID.vendorId.size();
smoritaemb 0:580aba13d1a1 299 const uint16_t locatorSize = sizeof(Locator);
smoritaemb 0:580aba13d1a1 300 const uint16_t durationSize =
smoritaemb 0:580aba13d1a1 301 sizeof(Duration_t::seconds) + sizeof(Duration_t::fraction);
smoritaemb 0:580aba13d1a1 302 const uint16_t entityKeySize = 3;
smoritaemb 0:580aba13d1a1 303 const uint16_t entityKindSize = 1;
smoritaemb 0:580aba13d1a1 304 const uint16_t entityIdSize = entityKeySize + entityKindSize;
smoritaemb 0:580aba13d1a1 305 const uint16_t guidSize = sizeof(GuidPrefix_t::id) + entityIdSize;
smoritaemb 0:580aba13d1a1 306
smoritaemb 0:580aba13d1a1 307 const Locator userUniCastLocator =
smoritaemb 0:580aba13d1a1 308 getUserUnicastLocator(mp_participant->m_participantId);
smoritaemb 0:580aba13d1a1 309 const Locator builtInUniCastLocator =
smoritaemb 0:580aba13d1a1 310 getBuiltInUnicastLocator(mp_participant->m_participantId);
smoritaemb 0:580aba13d1a1 311 const Locator builtInMultiCastLocator = getBuiltInMulticastLocator();
smoritaemb 0:580aba13d1a1 312
smoritaemb 0:580aba13d1a1 313 ucdr_serialize_array_uint8_t(&m_microbuffer,
smoritaemb 0:580aba13d1a1 314 rtps::SMElement::SCHEME_PL_CDR_LE.data(),
smoritaemb 0:580aba13d1a1 315 rtps::SMElement::SCHEME_PL_CDR_LE.size());
smoritaemb 0:580aba13d1a1 316 ucdr_serialize_uint16_t(&m_microbuffer, zero_options);
smoritaemb 0:580aba13d1a1 317
smoritaemb 0:580aba13d1a1 318 ucdr_serialize_uint16_t(&m_microbuffer, ParameterId::PID_PROTOCOL_VERSION);
smoritaemb 0:580aba13d1a1 319 ucdr_serialize_uint16_t(&m_microbuffer, protocolVersionSize + 2);
smoritaemb 0:580aba13d1a1 320 ucdr_serialize_uint8_t(&m_microbuffer, PROTOCOLVERSION.major);
smoritaemb 0:580aba13d1a1 321 ucdr_serialize_uint8_t(&m_microbuffer, PROTOCOLVERSION.minor);
smoritaemb 0:580aba13d1a1 322 m_microbuffer.iterator += 2; // padding
smoritaemb 0:580aba13d1a1 323 m_microbuffer.last_data_size = 4; // to 4 byte
smoritaemb 0:580aba13d1a1 324
smoritaemb 0:580aba13d1a1 325 ucdr_serialize_uint16_t(&m_microbuffer, ParameterId::PID_VENDORID);
smoritaemb 0:580aba13d1a1 326 ucdr_serialize_uint16_t(&m_microbuffer, vendorIdSize + 2);
smoritaemb 0:580aba13d1a1 327 ucdr_serialize_array_uint8_t(&m_microbuffer,
smoritaemb 0:580aba13d1a1 328 Config::VENDOR_ID.vendorId.data(), vendorIdSize);
smoritaemb 0:580aba13d1a1 329 m_microbuffer.iterator += 2; // padding
smoritaemb 0:580aba13d1a1 330 m_microbuffer.last_data_size = 4; // to 4 byte
smoritaemb 0:580aba13d1a1 331
smoritaemb 0:580aba13d1a1 332 ucdr_serialize_uint16_t(&m_microbuffer,
smoritaemb 0:580aba13d1a1 333 ParameterId::PID_DEFAULT_UNICAST_LOCATOR);
smoritaemb 0:580aba13d1a1 334 ucdr_serialize_uint16_t(&m_microbuffer, locatorSize);
smoritaemb 0:580aba13d1a1 335 ucdr_serialize_array_uint8_t(
smoritaemb 0:580aba13d1a1 336 &m_microbuffer, reinterpret_cast<const uint8_t *>(&userUniCastLocator),
smoritaemb 0:580aba13d1a1 337 locatorSize);
smoritaemb 0:580aba13d1a1 338
smoritaemb 0:580aba13d1a1 339 ucdr_serialize_uint16_t(&m_microbuffer,
smoritaemb 0:580aba13d1a1 340 ParameterId::PID_METATRAFFIC_UNICAST_LOCATOR);
smoritaemb 0:580aba13d1a1 341 ucdr_serialize_uint16_t(&m_microbuffer, locatorSize);
smoritaemb 0:580aba13d1a1 342 ucdr_serialize_array_uint8_t(
smoritaemb 0:580aba13d1a1 343 &m_microbuffer, reinterpret_cast<const uint8_t *>(&builtInUniCastLocator),
smoritaemb 0:580aba13d1a1 344 locatorSize);
smoritaemb 0:580aba13d1a1 345
smoritaemb 0:580aba13d1a1 346 ucdr_serialize_uint16_t(&m_microbuffer,
smoritaemb 0:580aba13d1a1 347 ParameterId::PID_METATRAFFIC_MULTICAST_LOCATOR);
smoritaemb 0:580aba13d1a1 348 ucdr_serialize_uint16_t(&m_microbuffer, locatorSize);
smoritaemb 0:580aba13d1a1 349 ucdr_serialize_array_uint8_t(
smoritaemb 0:580aba13d1a1 350 &m_microbuffer,
smoritaemb 0:580aba13d1a1 351 reinterpret_cast<const uint8_t *>(&builtInMultiCastLocator), locatorSize);
smoritaemb 0:580aba13d1a1 352
smoritaemb 0:580aba13d1a1 353 ucdr_serialize_uint16_t(&m_microbuffer,
smoritaemb 0:580aba13d1a1 354 ParameterId::PID_PARTICIPANT_LEASE_DURATION);
smoritaemb 0:580aba13d1a1 355 ucdr_serialize_uint16_t(&m_microbuffer, durationSize);
smoritaemb 0:580aba13d1a1 356 ucdr_serialize_int32_t(&m_microbuffer, Config::SPDP_LEASE_DURATION.seconds);
smoritaemb 0:580aba13d1a1 357 ucdr_serialize_uint32_t(&m_microbuffer, Config::SPDP_LEASE_DURATION.fraction);
smoritaemb 0:580aba13d1a1 358
smoritaemb 0:580aba13d1a1 359 ucdr_serialize_uint16_t(&m_microbuffer, ParameterId::PID_PARTICIPANT_GUID);
smoritaemb 0:580aba13d1a1 360 ucdr_serialize_uint16_t(&m_microbuffer, guidSize);
smoritaemb 0:580aba13d1a1 361 ucdr_serialize_array_uint8_t(&m_microbuffer,
smoritaemb 0:580aba13d1a1 362 mp_participant->m_guidPrefix.id.data(),
smoritaemb 0:580aba13d1a1 363 sizeof(GuidPrefix_t::id));
smoritaemb 0:580aba13d1a1 364 ucdr_serialize_array_uint8_t(&m_microbuffer,
smoritaemb 0:580aba13d1a1 365 ENTITYID_BUILD_IN_PARTICIPANT.entityKey.data(),
smoritaemb 0:580aba13d1a1 366 entityKeySize);
smoritaemb 0:580aba13d1a1 367 ucdr_serialize_uint8_t(
smoritaemb 0:580aba13d1a1 368 &m_microbuffer,
smoritaemb 0:580aba13d1a1 369 static_cast<uint8_t>(ENTITYID_BUILD_IN_PARTICIPANT.entityKind));
smoritaemb 0:580aba13d1a1 370
smoritaemb 0:580aba13d1a1 371 ucdr_serialize_uint16_t(&m_microbuffer,
smoritaemb 0:580aba13d1a1 372 ParameterId::PID_BUILTIN_ENDPOINT_SET);
smoritaemb 0:580aba13d1a1 373 ucdr_serialize_uint16_t(&m_microbuffer, sizeof(BuildInEndpointSet));
smoritaemb 0:580aba13d1a1 374 ucdr_serialize_uint32_t(
smoritaemb 0:580aba13d1a1 375 &m_microbuffer, BuildInEndpointSet::DISC_BIE_PARTICIPANT_ANNOUNCER |
smoritaemb 0:580aba13d1a1 376 BuildInEndpointSet::DISC_BIE_PARTICIPANT_DETECTOR |
smoritaemb 0:580aba13d1a1 377 BuildInEndpointSet::DISC_BIE_PUBLICATION_ANNOUNCER |
smoritaemb 0:580aba13d1a1 378 BuildInEndpointSet::DISC_BIE_PUBLICATION_DETECTOR |
smoritaemb 0:580aba13d1a1 379 BuildInEndpointSet::DISC_BIE_SUBSCRIPTION_ANNOUNCER |
smoritaemb 0:580aba13d1a1 380 BuildInEndpointSet::DISC_BIE_SUBSCRIPTION_DETECTOR);
smoritaemb 0:580aba13d1a1 381
smoritaemb 0:580aba13d1a1 382 endCurrentList();
smoritaemb 0:580aba13d1a1 383 }
smoritaemb 0:580aba13d1a1 384
smoritaemb 0:580aba13d1a1 385 void callRunBroadcast(void *arg){
smoritaemb 0:580aba13d1a1 386 SPDPAgent::runBroadcast(arg);
smoritaemb 0:580aba13d1a1 387 }
smoritaemb 0:580aba13d1a1 388
smoritaemb 0:580aba13d1a1 389 #undef SPDP_VERBOSE