S Morita / mbed-mros2

Porting mros2 as an Mbed library.

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

embeddedRTPS/include/rtps/entities/StatefulReader.tpp@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 "lwip/tcpip.h"
smoritaemb 0:580aba13d1a1 26 #include "rtps/entities/StatefulReader.h"
smoritaemb 0:580aba13d1a1 27 #include "rtps/messages/MessageFactory.h"
smoritaemb 0:580aba13d1a1 28 #include "rtps/utils/Lock.h"
smoritaemb 0:580aba13d1a1 29
smoritaemb 0:580aba13d1a1 30 #define SFR_VERBOSE 0
smoritaemb 0:580aba13d1a1 31
smoritaemb 0:580aba13d1a1 32 #if SFR_VERBOSE
smoritaemb 0:580aba13d1a1 33 #include "rtps/utils/printutils.h"
smoritaemb 0:580aba13d1a1 34 #endif
smoritaemb 0:580aba13d1a1 35
smoritaemb 0:580aba13d1a1 36 using rtps::StatefulReaderT;
smoritaemb 0:580aba13d1a1 37
smoritaemb 0:580aba13d1a1 38 template <class NetworkDriver>
smoritaemb 0:580aba13d1a1 39 StatefulReaderT<NetworkDriver>::~StatefulReaderT() {
smoritaemb 0:580aba13d1a1 40 // if(sys_mutex_valid(&m_mutex)){
smoritaemb 0:580aba13d1a1 41 sys_mutex_free(&m_mutex);
smoritaemb 0:580aba13d1a1 42 //}
smoritaemb 0:580aba13d1a1 43 }
smoritaemb 0:580aba13d1a1 44
smoritaemb 0:580aba13d1a1 45 template <class NetworkDriver>
smoritaemb 0:580aba13d1a1 46 void StatefulReaderT<NetworkDriver>::init(const TopicData &attributes,
smoritaemb 0:580aba13d1a1 47 NetworkDriver &driver) {
smoritaemb 0:580aba13d1a1 48 m_attributes = attributes;
smoritaemb 0:580aba13d1a1 49 m_transport = &driver;
smoritaemb 0:580aba13d1a1 50 m_packetInfo.srcPort = attributes.unicastLocator.port;
smoritaemb 0:580aba13d1a1 51 sys_mutex_new(&m_mutex);
smoritaemb 0:580aba13d1a1 52 m_is_initialized_ = true;
smoritaemb 0:580aba13d1a1 53 }
smoritaemb 0:580aba13d1a1 54
smoritaemb 0:580aba13d1a1 55 template <class NetworkDriver>
smoritaemb 0:580aba13d1a1 56 void StatefulReaderT<NetworkDriver>::newChange(
smoritaemb 0:580aba13d1a1 57 const ReaderCacheChange &cacheChange) {
smoritaemb 0:580aba13d1a1 58 if (m_callback == nullptr) {
smoritaemb 0:580aba13d1a1 59 return;
smoritaemb 0:580aba13d1a1 60 }
smoritaemb 0:580aba13d1a1 61 Lock lock{m_mutex};
smoritaemb 0:580aba13d1a1 62 for (auto &proxy : m_proxies) {
smoritaemb 0:580aba13d1a1 63 if (proxy.remoteWriterGuid == cacheChange.writerGuid) {
smoritaemb 0:580aba13d1a1 64 //if (proxy.expectedSN == cacheChange.sn) {
smoritaemb 0:580aba13d1a1 65 m_callback(m_callee, cacheChange);
smoritaemb 0:580aba13d1a1 66 ++proxy.expectedSN;
smoritaemb 0:580aba13d1a1 67 return;
smoritaemb 0:580aba13d1a1 68 //}
smoritaemb 0:580aba13d1a1 69 }
smoritaemb 0:580aba13d1a1 70 }
smoritaemb 0:580aba13d1a1 71 }
smoritaemb 0:580aba13d1a1 72
smoritaemb 0:580aba13d1a1 73 template <class NetworkDriver>
smoritaemb 0:580aba13d1a1 74 void StatefulReaderT<NetworkDriver>::registerCallback(ddsReaderCallback_fp cb,
smoritaemb 0:580aba13d1a1 75 void *callee) {
smoritaemb 0:580aba13d1a1 76 if (cb != nullptr) {
smoritaemb 0:580aba13d1a1 77 m_callback = cb;
smoritaemb 0:580aba13d1a1 78 m_callee = callee; // It's okay if this is null
smoritaemb 0:580aba13d1a1 79 } else {
smoritaemb 0:580aba13d1a1 80 #if SLR_VERBOSE
smoritaemb 0:580aba13d1a1 81 printf("StatefulReader[%s]: Passed callback is nullptr\n",
smoritaemb 0:580aba13d1a1 82 &m_attributes.topicName[0]);
smoritaemb 0:580aba13d1a1 83 #endif
smoritaemb 0:580aba13d1a1 84 }
smoritaemb 0:580aba13d1a1 85 }
smoritaemb 0:580aba13d1a1 86
smoritaemb 0:580aba13d1a1 87 template <class NetworkDriver>
smoritaemb 0:580aba13d1a1 88 bool StatefulReaderT<NetworkDriver>::addNewMatchedWriter(
smoritaemb 0:580aba13d1a1 89 const WriterProxy &newProxy) {
smoritaemb 0:580aba13d1a1 90 #if SFR_VERBOSE
smoritaemb 0:580aba13d1a1 91 printf("StatefulReader[%s]: New writer added with id: ",
smoritaemb 0:580aba13d1a1 92 &this->m_attributes.topicName[0]);
smoritaemb 0:580aba13d1a1 93 printGuid(newProxy.remoteWriterGuid);
smoritaemb 0:580aba13d1a1 94 printf("\n");
smoritaemb 0:580aba13d1a1 95 #endif
smoritaemb 0:580aba13d1a1 96 return m_proxies.add(newProxy);
smoritaemb 0:580aba13d1a1 97 }
smoritaemb 0:580aba13d1a1 98
smoritaemb 0:580aba13d1a1 99 template <class NetworkDriver>
smoritaemb 0:580aba13d1a1 100 void StatefulReaderT<NetworkDriver>::removeWriter(const Guid &guid) {
smoritaemb 0:580aba13d1a1 101 auto isElementToRemove = [&](const WriterProxy &proxy) {
smoritaemb 0:580aba13d1a1 102 return proxy.remoteWriterGuid == guid;
smoritaemb 0:580aba13d1a1 103 };
smoritaemb 0:580aba13d1a1 104 auto thunk = [](void *arg, const WriterProxy &value) {
smoritaemb 0:580aba13d1a1 105 return (*static_cast<decltype(isElementToRemove) *>(arg))(value);
smoritaemb 0:580aba13d1a1 106 };
smoritaemb 0:580aba13d1a1 107
smoritaemb 0:580aba13d1a1 108 m_proxies.remove(thunk, &isElementToRemove);
smoritaemb 0:580aba13d1a1 109 }
smoritaemb 0:580aba13d1a1 110
smoritaemb 0:580aba13d1a1 111 template <class NetworkDriver>
smoritaemb 0:580aba13d1a1 112 bool StatefulReaderT<NetworkDriver>::onNewHeartbeat(
smoritaemb 0:580aba13d1a1 113 const SubmessageHeartbeat &msg, const GuidPrefix_t &sourceGuidPrefix) {
smoritaemb 0:580aba13d1a1 114 Lock lock(m_mutex);
smoritaemb 0:580aba13d1a1 115 PacketInfo info;
smoritaemb 0:580aba13d1a1 116 info.srcPort = m_packetInfo.srcPort;
smoritaemb 0:580aba13d1a1 117 WriterProxy *writer = nullptr;
smoritaemb 0:580aba13d1a1 118 // Search for writer
smoritaemb 0:580aba13d1a1 119 for (WriterProxy &proxy : m_proxies) {
smoritaemb 0:580aba13d1a1 120 if (proxy.remoteWriterGuid.prefix == sourceGuidPrefix &&
smoritaemb 0:580aba13d1a1 121 proxy.remoteWriterGuid.entityId == msg.writerId) {
smoritaemb 0:580aba13d1a1 122 writer = &proxy;
smoritaemb 0:580aba13d1a1 123 break;
smoritaemb 0:580aba13d1a1 124 }
smoritaemb 0:580aba13d1a1 125 }
smoritaemb 0:580aba13d1a1 126
smoritaemb 0:580aba13d1a1 127 if (writer == nullptr) {
smoritaemb 0:580aba13d1a1 128 #if SFR_VERBOSE
smoritaemb 0:580aba13d1a1 129 printf("StatefulReader[%s]: Ignore heartbeat. Couldn't find a matching "
smoritaemb 0:580aba13d1a1 130 "writer with id: ",
smoritaemb 0:580aba13d1a1 131 &this->m_attributes.topicName[0]);
smoritaemb 0:580aba13d1a1 132 printEntityId(msg.writerId);
smoritaemb 0:580aba13d1a1 133 printf("\n");
smoritaemb 0:580aba13d1a1 134 #endif
smoritaemb 0:580aba13d1a1 135 return false;
smoritaemb 0:580aba13d1a1 136 }
smoritaemb 0:580aba13d1a1 137
smoritaemb 0:580aba13d1a1 138 if (msg.count.value <= writer->hbCount.value) {
smoritaemb 0:580aba13d1a1 139 #if SFR_VERBOSE
smoritaemb 0:580aba13d1a1 140 printf("StatefulReader[%s]: Ignore heartbeat. Count too low.\n",
smoritaemb 0:580aba13d1a1 141 &this->m_attributes.topicName[0]);
smoritaemb 0:580aba13d1a1 142 #endif
smoritaemb 0:580aba13d1a1 143 return false;
smoritaemb 0:580aba13d1a1 144 }
smoritaemb 0:580aba13d1a1 145
smoritaemb 0:580aba13d1a1 146 writer->hbCount.value = msg.count.value;
smoritaemb 0:580aba13d1a1 147 info.destAddr = writer->remoteLocator.getIp4Address();
smoritaemb 0:580aba13d1a1 148 info.destPort = writer->remoteLocator.port;
smoritaemb 0:580aba13d1a1 149 rtps::MessageFactory::addHeader(info.buffer,
smoritaemb 0:580aba13d1a1 150 m_attributes.endpointGuid.prefix);
smoritaemb 0:580aba13d1a1 151 rtps::MessageFactory::addSubMessageDestination(info.buffer);
smoritaemb 0:580aba13d1a1 152 rtps::MessageFactory::addAckNack(info.buffer, msg.writerId, msg.readerId,
smoritaemb 0:580aba13d1a1 153 writer->getMissing(msg.firstSN, msg.lastSN),
smoritaemb 0:580aba13d1a1 154 writer->getNextAckNackCount());
smoritaemb 0:580aba13d1a1 155
smoritaemb 0:580aba13d1a1 156 #if SFR_VERBOSE
smoritaemb 0:580aba13d1a1 157 printf("StatefulReader[%s]: Sending acknack.\n",
smoritaemb 0:580aba13d1a1 158 &this->m_attributes.topicName[0]);
smoritaemb 0:580aba13d1a1 159 #endif
smoritaemb 0:580aba13d1a1 160 m_transport->sendPacket(info);
smoritaemb 0:580aba13d1a1 161 return true;
smoritaemb 0:580aba13d1a1 162 }
smoritaemb 0:580aba13d1a1 163
smoritaemb 0:580aba13d1a1 164 #undef SFR_VERBOSE