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/entities/Participant.cpp@0:580aba13d1a1, 2021-12-30 (annotated)

Committer:
smoritaemb
Date:
Thu Dec 30 21:06:29 2021 +0900
Revision:
0:580aba13d1a1
Updated to catch up to mros2 v2.3

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/entities/Participant.h"
smoritaemb 0:580aba13d1a1 26
smoritaemb 0:580aba13d1a1 27 #include "rtps/entities/Reader.h"
smoritaemb 0:580aba13d1a1 28 #include "rtps/entities/Writer.h"
smoritaemb 0:580aba13d1a1 29 #include "rtps/messages/MessageReceiver.h"
smoritaemb 0:580aba13d1a1 30
smoritaemb 0:580aba13d1a1 31 using rtps::Participant;
smoritaemb 0:580aba13d1a1 32
smoritaemb 0:580aba13d1a1 33 Participant::Participant()
smoritaemb 0:580aba13d1a1 34 : m_guidPrefix(GUIDPREFIX_UNKNOWN), m_participantId(PARTICIPANT_ID_INVALID),
smoritaemb 0:580aba13d1a1 35 m_receiver(this) {}
smoritaemb 0:580aba13d1a1 36 Participant::Participant(const GuidPrefix_t &guidPrefix,
smoritaemb 0:580aba13d1a1 37 ParticipantId_t participantId)
smoritaemb 0:580aba13d1a1 38 : m_guidPrefix(guidPrefix), m_participantId(participantId),
smoritaemb 0:580aba13d1a1 39 m_receiver(this) {}
smoritaemb 0:580aba13d1a1 40
smoritaemb 0:580aba13d1a1 41 Participant::~Participant() { m_spdpAgent.stop(); }
smoritaemb 0:580aba13d1a1 42
smoritaemb 0:580aba13d1a1 43 void Participant::reuse(const GuidPrefix_t &guidPrefix,
smoritaemb 0:580aba13d1a1 44 ParticipantId_t participantId) {
smoritaemb 0:580aba13d1a1 45 m_guidPrefix = guidPrefix;
smoritaemb 0:580aba13d1a1 46 m_participantId = participantId;
smoritaemb 0:580aba13d1a1 47 }
smoritaemb 0:580aba13d1a1 48
smoritaemb 0:580aba13d1a1 49 bool Participant::isValid() {
smoritaemb 0:580aba13d1a1 50 return m_participantId != PARTICIPANT_ID_INVALID;
smoritaemb 0:580aba13d1a1 51 }
smoritaemb 0:580aba13d1a1 52
smoritaemb 0:580aba13d1a1 53 std::array<uint8_t, 3> Participant::getNextUserEntityKey() {
smoritaemb 0:580aba13d1a1 54 const auto result = m_nextUserEntityId;
smoritaemb 0:580aba13d1a1 55
smoritaemb 0:580aba13d1a1 56 ++m_nextUserEntityId[2];
smoritaemb 0:580aba13d1a1 57 if (m_nextUserEntityId[2] == 0) {
smoritaemb 0:580aba13d1a1 58 ++m_nextUserEntityId[1];
smoritaemb 0:580aba13d1a1 59 if (m_nextUserEntityId[1] == 0) {
smoritaemb 0:580aba13d1a1 60 ++m_nextUserEntityId[0];
smoritaemb 0:580aba13d1a1 61 }
smoritaemb 0:580aba13d1a1 62 }
smoritaemb 0:580aba13d1a1 63 return result;
smoritaemb 0:580aba13d1a1 64 }
smoritaemb 0:580aba13d1a1 65
smoritaemb 0:580aba13d1a1 66 bool Participant::registerOnNewPublisherMatchedCallback(
smoritaemb 0:580aba13d1a1 67 void (*callback)(void *arg), void *args) {
smoritaemb 0:580aba13d1a1 68 if (!m_hasBuilInEndpoints) {
smoritaemb 0:580aba13d1a1 69 return false;
smoritaemb 0:580aba13d1a1 70 }
smoritaemb 0:580aba13d1a1 71
smoritaemb 0:580aba13d1a1 72 m_sedpAgent.registerOnNewPublisherMatchedCallback(callback, args);
smoritaemb 0:580aba13d1a1 73 return true;
smoritaemb 0:580aba13d1a1 74 }
smoritaemb 0:580aba13d1a1 75
smoritaemb 0:580aba13d1a1 76 bool Participant::registerOnNewSubscriberMatchedCallback(
smoritaemb 0:580aba13d1a1 77 void (*callback)(void *arg), void *args) {
smoritaemb 0:580aba13d1a1 78 if (!m_hasBuilInEndpoints) {
smoritaemb 0:580aba13d1a1 79 return false;
smoritaemb 0:580aba13d1a1 80 }
smoritaemb 0:580aba13d1a1 81
smoritaemb 0:580aba13d1a1 82 m_sedpAgent.registerOnNewSubscriberMatchedCallback(callback, args);
smoritaemb 0:580aba13d1a1 83 return true;
smoritaemb 0:580aba13d1a1 84 }
smoritaemb 0:580aba13d1a1 85
smoritaemb 0:580aba13d1a1 86 rtps::Writer *Participant::addWriter(Writer *pWriter) {
smoritaemb 0:580aba13d1a1 87 if (pWriter != nullptr && m_numWriters != m_writers.size()) {
smoritaemb 0:580aba13d1a1 88 m_writers[m_numWriters++] = pWriter;
smoritaemb 0:580aba13d1a1 89 if (m_hasBuilInEndpoints) {
smoritaemb 0:580aba13d1a1 90 m_sedpAgent.addWriter(*pWriter);
smoritaemb 0:580aba13d1a1 91 }
smoritaemb 0:580aba13d1a1 92 return pWriter;
smoritaemb 0:580aba13d1a1 93 } else {
smoritaemb 0:580aba13d1a1 94 return nullptr;
smoritaemb 0:580aba13d1a1 95 }
smoritaemb 0:580aba13d1a1 96 }
smoritaemb 0:580aba13d1a1 97
smoritaemb 0:580aba13d1a1 98 bool Participant::isWritersFull() { return m_numWriters == m_writers.size(); }
smoritaemb 0:580aba13d1a1 99
smoritaemb 0:580aba13d1a1 100 rtps::Reader *Participant::addReader(Reader *pReader) {
smoritaemb 0:580aba13d1a1 101 if (pReader != nullptr && m_numReaders != m_readers.size()) {
smoritaemb 0:580aba13d1a1 102 m_readers[m_numReaders++] = pReader;
smoritaemb 0:580aba13d1a1 103 if (m_hasBuilInEndpoints) {
smoritaemb 0:580aba13d1a1 104 m_sedpAgent.addReader(*pReader);
smoritaemb 0:580aba13d1a1 105 }
smoritaemb 0:580aba13d1a1 106 return pReader;
smoritaemb 0:580aba13d1a1 107 } else {
smoritaemb 0:580aba13d1a1 108 return nullptr;
smoritaemb 0:580aba13d1a1 109 }
smoritaemb 0:580aba13d1a1 110 }
smoritaemb 0:580aba13d1a1 111
smoritaemb 0:580aba13d1a1 112 bool Participant::isReadersFull() { return m_numReaders == m_readers.size(); }
smoritaemb 0:580aba13d1a1 113
smoritaemb 0:580aba13d1a1 114 rtps::Writer *Participant::getWriter(EntityId_t id) const {
smoritaemb 0:580aba13d1a1 115 for (uint8_t i = 0; i < m_numWriters; ++i) {
smoritaemb 0:580aba13d1a1 116 if (m_writers[i]->m_attributes.endpointGuid.entityId == id) {
smoritaemb 0:580aba13d1a1 117 return m_writers[i];
smoritaemb 0:580aba13d1a1 118 }
smoritaemb 0:580aba13d1a1 119 }
smoritaemb 0:580aba13d1a1 120 return nullptr;
smoritaemb 0:580aba13d1a1 121 }
smoritaemb 0:580aba13d1a1 122
smoritaemb 0:580aba13d1a1 123 rtps::Reader *Participant::getReader(EntityId_t id) const {
smoritaemb 0:580aba13d1a1 124 for (uint8_t i = 0; i < m_numReaders; ++i) {
smoritaemb 0:580aba13d1a1 125 if (m_readers[i]->m_attributes.endpointGuid.entityId == id) {
smoritaemb 0:580aba13d1a1 126 return m_readers[i];
smoritaemb 0:580aba13d1a1 127 }
smoritaemb 0:580aba13d1a1 128 }
smoritaemb 0:580aba13d1a1 129 return nullptr;
smoritaemb 0:580aba13d1a1 130 }
smoritaemb 0:580aba13d1a1 131
smoritaemb 0:580aba13d1a1 132 rtps::Writer *
smoritaemb 0:580aba13d1a1 133 Participant::getMatchingWriter(const TopicData &readerTopicData) const {
smoritaemb 0:580aba13d1a1 134 for (uint8_t i = 0; i < m_numWriters; ++i) {
smoritaemb 0:580aba13d1a1 135 if (m_writers[i]->m_attributes.matchesTopicOf(readerTopicData) &&
smoritaemb 0:580aba13d1a1 136 (readerTopicData.reliabilityKind == ReliabilityKind_t::BEST_EFFORT ||
smoritaemb 0:580aba13d1a1 137 m_writers[i]->m_attributes.reliabilityKind ==
smoritaemb 0:580aba13d1a1 138 ReliabilityKind_t::RELIABLE)) {
smoritaemb 0:580aba13d1a1 139 return m_writers[i];
smoritaemb 0:580aba13d1a1 140 }
smoritaemb 0:580aba13d1a1 141 }
smoritaemb 0:580aba13d1a1 142 return nullptr;
smoritaemb 0:580aba13d1a1 143 }
smoritaemb 0:580aba13d1a1 144
smoritaemb 0:580aba13d1a1 145 rtps::Reader *
smoritaemb 0:580aba13d1a1 146 Participant::getMatchingReader(const TopicData &writerTopicData) const {
smoritaemb 0:580aba13d1a1 147 for (uint8_t i = 0; i < m_numReaders; ++i) {
smoritaemb 0:580aba13d1a1 148 if (m_readers[i]->m_attributes.matchesTopicOf(writerTopicData) &&
smoritaemb 0:580aba13d1a1 149 (writerTopicData.reliabilityKind == ReliabilityKind_t::RELIABLE ||
smoritaemb 0:580aba13d1a1 150 m_readers[i]->m_attributes.reliabilityKind ==
smoritaemb 0:580aba13d1a1 151 ReliabilityKind_t::BEST_EFFORT)) {
smoritaemb 0:580aba13d1a1 152 return m_readers[i];
smoritaemb 0:580aba13d1a1 153 }
smoritaemb 0:580aba13d1a1 154 }
smoritaemb 0:580aba13d1a1 155 return nullptr;
smoritaemb 0:580aba13d1a1 156 }
smoritaemb 0:580aba13d1a1 157
smoritaemb 0:580aba13d1a1 158 bool Participant::addNewRemoteParticipant(
smoritaemb 0:580aba13d1a1 159 const ParticipantProxyData &remotePart) {
smoritaemb 0:580aba13d1a1 160 return m_remoteParticipants.add(remotePart);
smoritaemb 0:580aba13d1a1 161 }
smoritaemb 0:580aba13d1a1 162
smoritaemb 0:580aba13d1a1 163 bool Participant::removeRemoteParticipant(const GuidPrefix_t &prefix) {
smoritaemb 0:580aba13d1a1 164 auto isElementToRemove = [&](const ParticipantProxyData &proxy) {
smoritaemb 0:580aba13d1a1 165 return proxy.m_guid.prefix == prefix;
smoritaemb 0:580aba13d1a1 166 };
smoritaemb 0:580aba13d1a1 167 auto thunk = [](void *arg, const ParticipantProxyData &value) {
smoritaemb 0:580aba13d1a1 168 return (*static_cast<decltype(isElementToRemove) *>(arg))(value);
smoritaemb 0:580aba13d1a1 169 };
smoritaemb 0:580aba13d1a1 170
smoritaemb 0:580aba13d1a1 171 return m_remoteParticipants.remove(thunk, &isElementToRemove);
smoritaemb 0:580aba13d1a1 172 }
smoritaemb 0:580aba13d1a1 173
smoritaemb 0:580aba13d1a1 174 const rtps::ParticipantProxyData *
smoritaemb 0:580aba13d1a1 175 Participant::findRemoteParticipant(const GuidPrefix_t &prefix) {
smoritaemb 0:580aba13d1a1 176 auto isElementToFind = [&](const ParticipantProxyData &proxy) {
smoritaemb 0:580aba13d1a1 177 return proxy.m_guid.prefix == prefix;
smoritaemb 0:580aba13d1a1 178 };
smoritaemb 0:580aba13d1a1 179 auto thunk = [](void *arg, const ParticipantProxyData &value) {
smoritaemb 0:580aba13d1a1 180 return (*static_cast<decltype(isElementToFind) *>(arg))(value);
smoritaemb 0:580aba13d1a1 181 };
smoritaemb 0:580aba13d1a1 182
smoritaemb 0:580aba13d1a1 183 return m_remoteParticipants.find(thunk, &isElementToFind);
smoritaemb 0:580aba13d1a1 184 }
smoritaemb 0:580aba13d1a1 185
smoritaemb 0:580aba13d1a1 186 uint32_t Participant::getRemoteParticipantCount() {
smoritaemb 0:580aba13d1a1 187 return m_remoteParticipants.getNumElements();
smoritaemb 0:580aba13d1a1 188 }
smoritaemb 0:580aba13d1a1 189
smoritaemb 0:580aba13d1a1 190 rtps::MessageReceiver *Participant::getMessageReceiver() { return &m_receiver; }
smoritaemb 0:580aba13d1a1 191
smoritaemb 0:580aba13d1a1 192 void Participant::addBuiltInEndpoints(BuiltInEndpoints &endpoints) {
smoritaemb 0:580aba13d1a1 193 m_hasBuilInEndpoints = true;
smoritaemb 0:580aba13d1a1 194 m_spdpAgent.init(*this, endpoints);
smoritaemb 0:580aba13d1a1 195 m_sedpAgent.init(*this, endpoints);
smoritaemb 0:580aba13d1a1 196
smoritaemb 0:580aba13d1a1 197 // This needs to be done after initializing the agents
smoritaemb 0:580aba13d1a1 198 addWriter(endpoints.spdpWriter);
smoritaemb 0:580aba13d1a1 199 addReader(endpoints.spdpReader);
smoritaemb 0:580aba13d1a1 200 addWriter(endpoints.sedpPubWriter);
smoritaemb 0:580aba13d1a1 201 addReader(endpoints.sedpPubReader);
smoritaemb 0:580aba13d1a1 202 addWriter(endpoints.sedpSubWriter);
smoritaemb 0:580aba13d1a1 203 addReader(endpoints.sedpSubReader);
smoritaemb 0:580aba13d1a1 204
smoritaemb 0:580aba13d1a1 205 m_spdpAgent.start();
smoritaemb 0:580aba13d1a1 206 }
smoritaemb 0:580aba13d1a1 207
smoritaemb 0:580aba13d1a1 208 void Participant::newMessage(const uint8_t *data, DataSize_t size) {
smoritaemb 0:580aba13d1a1 209 m_receiver.processMessage(data, size);
smoritaemb 0:580aba13d1a1 210 }