From 4fd7010295df70983851858d88eb86ba9e5a77c1 Mon Sep 17 00:00:00 2001 From: Sven Marbaise Date: Tue, 20 Jul 2021 00:30:38 +0200 Subject: [PATCH 01/25] works with chibios step1 --- include/rtps/common/types.h | 2 +- include/rtps/communication/UdpConnection.h | 4 ++++ include/rtps/discovery/ParticipantProxyData.h | 20 ++++++++++++++++--- include/rtps/discovery/SPDPAgent.h | 2 +- include/rtps/discovery/TopicData.h | 2 +- src/ThreadPool.cpp | 12 +---------- src/communication/UdpDriver.cpp | 2 +- src/discovery/TopicData.cpp | 2 +- 8 files changed, 27 insertions(+), 19 deletions(-) diff --git a/include/rtps/common/types.h b/include/rtps/common/types.h index 66204bf0..8c1304cf 100644 --- a/include/rtps/common/types.h +++ b/include/rtps/common/types.h @@ -273,7 +273,7 @@ const SequenceNumber_t SEQUENCENUMBER_UNKNOWN = {-1, 0}; const Time_t TIME_ZERO = {}; const Time_t TIME_INVALID = {-1, 0xFFFFFFFF}; -const Time_t TIME_INFINITE = {0x7FFFFFFF, 0xFFFFFFFF}; +//const Time_t TIME_INFINITE = {0x7FFFFFFF, 0xFFFFFFFF}; const VendorId_t VENDOR_UNKNOWN = {}; } // namespace rtps diff --git a/include/rtps/communication/UdpConnection.h b/include/rtps/communication/UdpConnection.h index 25b4fb5a..4368c208 100644 --- a/include/rtps/communication/UdpConnection.h +++ b/include/rtps/communication/UdpConnection.h @@ -31,7 +31,11 @@ Author: i11 - Embedded Software, RWTH Aachen University namespace rtps { struct UdpConnection { +#ifdef CHIBIOS + udp_pcb *pcb = NULL; +#else udp_pcb *pcb = nullptr; +#endif uint16_t port = 0; UdpConnection() = default; // Required for static allocation diff --git a/include/rtps/discovery/ParticipantProxyData.h b/include/rtps/discovery/ParticipantProxyData.h index 019484d3..ec62b553 100644 --- a/include/rtps/discovery/ParticipantProxyData.h +++ b/include/rtps/discovery/ParticipantProxyData.h @@ -28,9 +28,16 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/common/Locator.h" #include "rtps/config.h" #include "rtps/messages/MessageTypes.h" + +#ifdef CHIBIOS +#include +#else #include "ucdr/microcdr.h" +#endif #if defined(unix) || defined(__unix__) #include +#elif defined(CHIBIOS) +//#include "chtime.h" #endif #include @@ -63,8 +70,10 @@ class ParticipantProxyData { #if defined(unix) || defined(__unix__) std::chrono::time_point m_lastLivelinessReceivedTimestamp; -#else +#elif !defined(CHIBIOS) TickType_t m_lastLivelinessReceivedTickCount = 0; +#else + sysinterval_t m_lastLivelinessReceivedTickCount = 0; #endif void reset(); @@ -146,8 +155,10 @@ bool ParticipantProxyData::hasSubscriptionReader() { void ParticipantProxyData::onAliveSignal() { #if defined(unix) || defined(__unix__) m_lastLivelinessReceivedTimestamp = std::chrono::high_resolution_clock::now(); -#else +#elif !defined(CHIBIOS) m_lastLivelinessReceivedTickCount = xTaskGetTickCount(); +#else + m_lastLivelinessReceivedTickCount = chVTGetSystemTimeX(); #endif } @@ -157,9 +168,12 @@ uint32_t ParticipantProxyData::getAliveSignalAgeInMilliseconds() { std::chrono::duration duration = now - m_lastLivelinessReceivedTimestamp; return duration.count(); -#else +#elif !defined(CHIBIOS) return (xTaskGetTickCount() - m_lastLivelinessReceivedTickCount) * (1000 / configTICK_RATE_HZ); +#else + return (chVTGetSystemTimeX() - m_lastLivelinessReceivedTickCount) * + (1000 / 1); #endif } diff --git a/include/rtps/discovery/SPDPAgent.h b/include/rtps/discovery/SPDPAgent.h index faec555b..510b0d4e 100644 --- a/include/rtps/discovery/SPDPAgent.h +++ b/include/rtps/discovery/SPDPAgent.h @@ -30,7 +30,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/config.h" #include "rtps/discovery/BuiltInEndpoints.h" #include "rtps/discovery/ParticipantProxyData.h" -#include "ucdr/microcdr.h" +//#include "ucdr/microcdr.h" namespace rtps { class Participant; diff --git a/include/rtps/discovery/TopicData.h b/include/rtps/discovery/TopicData.h index 27165cf3..2b52b2e9 100644 --- a/include/rtps/discovery/TopicData.h +++ b/include/rtps/discovery/TopicData.h @@ -29,7 +29,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/common/Locator.h" #include "rtps/config.h" -#include "ucdr/microcdr.h" +//#include "ucdr/microcdr.h" #include namespace rtps { diff --git a/src/ThreadPool.cpp b/src/ThreadPool.cpp index d3606105..24002c12 100644 --- a/src/ThreadPool.cpp +++ b/src/ThreadPool.cpp @@ -161,27 +161,17 @@ void ThreadPool::doWriterWork() { } } - void ThreadPool::readCallback(void *args, udp_pcb *target, pbuf *pbuf, const ip_addr_t * /*addr*/, Ip4Port_t port) { auto &pool = *static_cast(args); PacketInfo packet; - - // TODO This is a workaround for chained pbufs caused by hardware limitations, not a general fix - if(pbuf->next != nullptr){ - struct pbuf* test = pbuf_alloc(PBUF_RAW, pbuf->tot_len, PBUF_POOL); - pbuf_copy(test, pbuf); - pbuf_free(pbuf); - pbuf = test; - } - packet.destAddr = {0}; // not relevant packet.destPort = target->local_port; packet.srcPort = port; packet.buffer = PBufWrapper{pbuf}; - if (!pool.addNewPacket(std::move(packet))) { + THREAD_POOL_LOG("ThreadPool: dropped packet\n"); } } diff --git a/src/communication/UdpDriver.cpp b/src/communication/UdpDriver.cpp index 0b976730..fff8d196 100644 --- a/src/communication/UdpDriver.cpp +++ b/src/communication/UdpDriver.cpp @@ -49,7 +49,7 @@ UdpDriver::UdpDriver(rtps::UdpDriver::udpRxFunc_fp callback, void *args) const rtps::UdpConnection * UdpDriver::createUdpConnection(Ip4Port_t receivePort) { - for (uint8_t i = 0; i < m_numConns; ++i) { + for (uint8_t i = 0; i < m_numConns; ++i) { // @suppress("Type cannot be resolved") if (m_conns[i].port == receivePort) { return &m_conns[i]; } diff --git a/src/discovery/TopicData.cpp b/src/discovery/TopicData.cpp index e7243c33..c6b748c3 100644 --- a/src/discovery/TopicData.cpp +++ b/src/discovery/TopicData.cpp @@ -76,7 +76,7 @@ bool TopicData::readFromUcdrBuffer(ucdrBuffer &buffer) { break; case ParameterId::PID_UNICAST_LOCATOR: uLoc.readFromUcdrBuffer(buffer); - if (uLoc.kind == LocatorKind_t::LOCATOR_KIND_UDPv4 && uLoc.isSameSubnet()) { + if (uLoc.kind == LocatorKind_t::LOCATOR_KIND_UDPv4) { unicastLocator = uLoc; } break; From 63e163fd2bed1ed8dce2c47c47082ac2f7f88d31 Mon Sep 17 00:00:00 2001 From: Sven Marbaise Date: Wed, 25 Aug 2021 15:04:32 +0200 Subject: [PATCH 02/25] undo some changes --- include/rtps/common/types.h | 5 +++-- include/rtps/communication/UdpConnection.h | 8 ++++---- include/rtps/discovery/ParticipantProxyData.h | 2 +- include/rtps/discovery/SPDPAgent.h | 2 +- include/rtps/discovery/TopicData.h | 2 +- src/ThreadPool.cpp | 7 +++++++ src/discovery/TopicData.cpp | 2 +- 7 files changed, 18 insertions(+), 10 deletions(-) diff --git a/include/rtps/common/types.h b/include/rtps/common/types.h index 8c1304cf..ed0284df 100644 --- a/include/rtps/common/types.h +++ b/include/rtps/common/types.h @@ -273,8 +273,9 @@ const SequenceNumber_t SEQUENCENUMBER_UNKNOWN = {-1, 0}; const Time_t TIME_ZERO = {}; const Time_t TIME_INVALID = {-1, 0xFFFFFFFF}; -//const Time_t TIME_INFINITE = {0x7FFFFFFF, 0xFFFFFFFF}; - +#ifndef CHIBIOS //has its own TIME_INFINITE +const Time_t TIME_INFINITE = {0x7FFFFFFF, 0xFFFFFFFF}; +#endif const VendorId_t VENDOR_UNKNOWN = {}; } // namespace rtps diff --git a/include/rtps/communication/UdpConnection.h b/include/rtps/communication/UdpConnection.h index 4368c208..087a3a0b 100644 --- a/include/rtps/communication/UdpConnection.h +++ b/include/rtps/communication/UdpConnection.h @@ -31,11 +31,11 @@ Author: i11 - Embedded Software, RWTH Aachen University namespace rtps { struct UdpConnection { -#ifdef CHIBIOS - udp_pcb *pcb = NULL; -#else +//#ifdef CHIBIOS +// udp_pcb *pcb = NULL; +//#else udp_pcb *pcb = nullptr; -#endif +//#endif uint16_t port = 0; UdpConnection() = default; // Required for static allocation diff --git a/include/rtps/discovery/ParticipantProxyData.h b/include/rtps/discovery/ParticipantProxyData.h index ec62b553..1a7c3a68 100644 --- a/include/rtps/discovery/ParticipantProxyData.h +++ b/include/rtps/discovery/ParticipantProxyData.h @@ -37,7 +37,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #if defined(unix) || defined(__unix__) #include #elif defined(CHIBIOS) -//#include "chtime.h" +#include "chtime.h" #endif #include diff --git a/include/rtps/discovery/SPDPAgent.h b/include/rtps/discovery/SPDPAgent.h index 510b0d4e..faec555b 100644 --- a/include/rtps/discovery/SPDPAgent.h +++ b/include/rtps/discovery/SPDPAgent.h @@ -30,7 +30,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/config.h" #include "rtps/discovery/BuiltInEndpoints.h" #include "rtps/discovery/ParticipantProxyData.h" -//#include "ucdr/microcdr.h" +#include "ucdr/microcdr.h" namespace rtps { class Participant; diff --git a/include/rtps/discovery/TopicData.h b/include/rtps/discovery/TopicData.h index 2b52b2e9..27165cf3 100644 --- a/include/rtps/discovery/TopicData.h +++ b/include/rtps/discovery/TopicData.h @@ -29,7 +29,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/common/Locator.h" #include "rtps/config.h" -//#include "ucdr/microcdr.h" +#include "ucdr/microcdr.h" #include namespace rtps { diff --git a/src/ThreadPool.cpp b/src/ThreadPool.cpp index 24002c12..5390ce11 100644 --- a/src/ThreadPool.cpp +++ b/src/ThreadPool.cpp @@ -166,6 +166,13 @@ void ThreadPool::readCallback(void *args, udp_pcb *target, pbuf *pbuf, auto &pool = *static_cast(args); PacketInfo packet; + // TODO This is a workaround for chained pbufs caused by hardware limitations, not a general fix + if(pbuf->next != nullptr){ + struct pbuf* test = pbuf_alloc(PBUF_RAW, pbuf->tot_len, PBUF_POOL); + pbuf_copy(test, pbuf); + pbuf_free(pbuf); + pbuf = test; + } packet.destAddr = {0}; // not relevant packet.destPort = target->local_port; packet.srcPort = port; diff --git a/src/discovery/TopicData.cpp b/src/discovery/TopicData.cpp index c6b748c3..98b174c7 100644 --- a/src/discovery/TopicData.cpp +++ b/src/discovery/TopicData.cpp @@ -76,7 +76,7 @@ bool TopicData::readFromUcdrBuffer(ucdrBuffer &buffer) { break; case ParameterId::PID_UNICAST_LOCATOR: uLoc.readFromUcdrBuffer(buffer); - if (uLoc.kind == LocatorKind_t::LOCATOR_KIND_UDPv4) { + if (uLoc.kind == LocatorKind_t::LOCATOR_KIND_UDPv4&& uLoc.isSameSubnet()) { unicastLocator = uLoc; } break; From 9ee29bd77d542fd19984d125d25a4c398b69bd93 Mon Sep 17 00:00:00 2001 From: Sven Marbaise Date: Wed, 25 Aug 2021 18:04:31 +0200 Subject: [PATCH 03/25] changed time function --- include/rtps/communication/UdpConnection.h | 5 +---- include/rtps/discovery/ParticipantProxyData.h | 8 ++------ 2 files changed, 3 insertions(+), 10 deletions(-) diff --git a/include/rtps/communication/UdpConnection.h b/include/rtps/communication/UdpConnection.h index 087a3a0b..8770f1c1 100644 --- a/include/rtps/communication/UdpConnection.h +++ b/include/rtps/communication/UdpConnection.h @@ -31,11 +31,8 @@ Author: i11 - Embedded Software, RWTH Aachen University namespace rtps { struct UdpConnection { -//#ifdef CHIBIOS -// udp_pcb *pcb = NULL; -//#else udp_pcb *pcb = nullptr; -//#endif + uint16_t port = 0; UdpConnection() = default; // Required for static allocation diff --git a/include/rtps/discovery/ParticipantProxyData.h b/include/rtps/discovery/ParticipantProxyData.h index 1a7c3a68..3a3ab288 100644 --- a/include/rtps/discovery/ParticipantProxyData.h +++ b/include/rtps/discovery/ParticipantProxyData.h @@ -29,11 +29,8 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/config.h" #include "rtps/messages/MessageTypes.h" -#ifdef CHIBIOS -#include -#else #include "ucdr/microcdr.h" -#endif + #if defined(unix) || defined(__unix__) #include #elif defined(CHIBIOS) @@ -172,8 +169,7 @@ uint32_t ParticipantProxyData::getAliveSignalAgeInMilliseconds() { return (xTaskGetTickCount() - m_lastLivelinessReceivedTickCount) * (1000 / configTICK_RATE_HZ); #else - return (chVTGetSystemTimeX() - m_lastLivelinessReceivedTickCount) * - (1000 / 1); + return chTimeI2MS(chVTGetSystemTimeX() - m_lastLivelinessReceivedTickCount); #endif } From bf07eed40e64f096ca9e733db072d420036748ac Mon Sep 17 00:00:00 2001 From: Sven Marbaise Date: Wed, 1 Sep 2021 11:05:54 +0200 Subject: [PATCH 04/25] changed structure --- src/entities/Participant.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/entities/Participant.cpp b/src/entities/Participant.cpp index 39a8a463..c821f0c5 100644 --- a/src/entities/Participant.cpp +++ b/src/entities/Participant.cpp @@ -1,3 +1,4 @@ + /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University From 60b36d9cb49f145e425e7f5d1d4db9273e7ec2f5 Mon Sep 17 00:00:00 2001 From: sven Date: Tue, 7 Sep 2021 23:02:27 +0200 Subject: [PATCH 05/25] Test again, boards reader and writer dont match --- src/discovery/SPDPAgent.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/discovery/SPDPAgent.cpp b/src/discovery/SPDPAgent.cpp index 7cced79d..c5c254f6 100644 --- a/src/discovery/SPDPAgent.cpp +++ b/src/discovery/SPDPAgent.cpp @@ -65,7 +65,7 @@ void SPDPAgent::init(Participant &participant, BuiltInEndpoints &endpoints) { ucdr_init_buffer(&m_microbuffer, m_outputBuffer.data(), m_outputBuffer.size()); - // addInlineQos(); +// addInlineQos(); addParticipantParameters(); initialized = true; } From fca95398a156c505f7874b8f3bc7369b7fa80abc Mon Sep 17 00:00:00 2001 From: sven Date: Mon, 13 Sep 2021 14:51:41 +0200 Subject: [PATCH 06/25] clean up 1 --- src/communication/UdpDriver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/communication/UdpDriver.cpp b/src/communication/UdpDriver.cpp index fff8d196..0b976730 100644 --- a/src/communication/UdpDriver.cpp +++ b/src/communication/UdpDriver.cpp @@ -49,7 +49,7 @@ UdpDriver::UdpDriver(rtps::UdpDriver::udpRxFunc_fp callback, void *args) const rtps::UdpConnection * UdpDriver::createUdpConnection(Ip4Port_t receivePort) { - for (uint8_t i = 0; i < m_numConns; ++i) { // @suppress("Type cannot be resolved") + for (uint8_t i = 0; i < m_numConns; ++i) { if (m_conns[i].port == receivePort) { return &m_conns[i]; } From dacf0299d7d9ac36aebe14b82f2328850b3c49df Mon Sep 17 00:00:00 2001 From: sven Date: Thu, 23 Sep 2021 13:02:16 +0200 Subject: [PATCH 07/25] minor changes --- include/rtps/common/types.h | 7 +++++ include/rtps/discovery/TopicData.h | 19 ++++++++++++-- include/rtps/entities/Domain.h | 11 ++++++++ src/entities/Domain.cpp | 41 ++++++++++++++++++++++++++++++ 4 files changed, 76 insertions(+), 2 deletions(-) diff --git a/include/rtps/common/types.h b/include/rtps/common/types.h index ed0284df..61152b26 100644 --- a/include/rtps/common/types.h +++ b/include/rtps/common/types.h @@ -84,6 +84,13 @@ enum class DurabilityKind_t : uint32_t { PERSISTENT = 3 }; +enum class OwnershipKind_t : uint32_t{ + SHARED = 0, + EXCLUSIVE = 1 +}; + +typedef uint32_t OwnershipStrength; + struct GuidPrefix_t { std::array id; diff --git a/include/rtps/discovery/TopicData.h b/include/rtps/discovery/TopicData.h index 27165cf3..91799144 100644 --- a/include/rtps/discovery/TopicData.h +++ b/include/rtps/discovery/TopicData.h @@ -46,6 +46,8 @@ struct TopicData { DurabilityKind_t durabilityKind; Locator unicastLocator; Locator multicastLocator; + OwnershipKind_t ownership_Kind; + OwnershipStrength ownership_strenght; TopicData() : endpointGuid(GUID_UNKNOWN), typeName{'\0'}, topicName{'\0'}, @@ -55,13 +57,26 @@ struct TopicData { 192, 168, 0, 42, rtps::getUserUnicastPort(0)); unicastLocator = someLocator; multicastLocator = Locator(); + ownership_Kind = OwnershipKind_t::SHARED; + ownership_strenght = 0; }; TopicData(Guid_t guid, ReliabilityKind_t reliability, Locator loc) : endpointGuid(guid), typeName{'\0'}, topicName{'\0'}, reliabilityKind(reliability), - durabilityKind(DurabilityKind_t::TRANSIENT_LOCAL), unicastLocator(loc) { - } + durabilityKind(DurabilityKind_t::TRANSIENT_LOCAL), unicastLocator(loc) , + ownership_Kind(OwnershipKind_t::SHARED), ownership_strenght(0){ + }; + + TopicData(Guid_t guid, OwnershipKind_t kind, OwnershipStrength strength, Locator loc) + : endpointGuid(guid), typeName{'\0'}, topicName{'\0'}, + ownership_Kind(kind), ownership_strenght(strength), + durabilityKind(DurabilityKind_t::TRANSIENT_LOCAL), unicastLocator(loc) { + + if(ownership_Kind == OwnershipKind_t::EXCLUSIVE ) { + reliabilityKind(ReliabilityKind_t::RELIABLE); + } + }; bool matchesTopicOf(const TopicData &other); diff --git a/include/rtps/entities/Domain.h b/include/rtps/entities/Domain.h index 14ed9812..ec88833b 100644 --- a/include/rtps/entities/Domain.h +++ b/include/rtps/entities/Domain.h @@ -45,13 +45,24 @@ class Domain { void stop(); Participant *createParticipant(); + Writer *createWriter(Participant &part, const char *topicName, const char *typeName, bool reliable, bool enforceUnicast = false); + + Writer *createWriter(Participant &part, const char *topicName, + const char *typeName, OwnershipKind_t ownership_kind, OwnershipStrength ownership_strenght, + bool enforceUnicast = false); + + Reader *createReader(Participant &part, const char *topicName, const char *typeName, bool reliable, ip4_addr_t mcastaddress = {0}); + Reader *createReader(Participant &part, const char *topicName, + const char *typeName, OwnerShipKind_t ownership_Kind, + ip4_addr_t mcastaddress = {0}); + Writer *writerExists(Participant &part, const char *topicName, const char *typeName, bool reliable); Reader *readerExists(Participant &part, const char *topicName, diff --git a/src/entities/Domain.cpp b/src/entities/Domain.cpp index 54733e39..7f97f2bd 100644 --- a/src/entities/Domain.cpp +++ b/src/entities/Domain.cpp @@ -359,6 +359,47 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, } } +rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, + const char *typeName, OwnershipKind_t ownership_kind, OwnershipStrength ownership_strenght, + bool enforceUnicast){ + // Check if there is enough capacity for more writers Ownership currently just support reliable writer so its compatible with fastDDS (dont really get why thats a problem) + if (m_statefulWriters.size() <= m_numStatefulWriters) { + DOMAIN_LOG("No Writer created. Max Number of Writers reached.\n"); + return nullptr; + } + + // TODO Distinguish WithKey and NoKey (Also changes EntityKind) + TopicData attributes; + + if (strlen(topicName) > Config::MAX_TOPICNAME_LENGTH || + strlen(typeName) > Config::MAX_TYPENAME_LENGTH) { + return nullptr; + } + strcpy(attributes.topicName, topicName); + strcpy(attributes.typeName, typeName); + attributes.endpointGuid.prefix = part.m_guidPrefix; + attributes.endpointGuid.entityId = { + part.getNextUserEntityKey(), + EntityKind_t::USER_DEFINED_WRITER_WITHOUT_KEY}; + attributes.unicastLocator = getUserUnicastLocator(part.m_participantId); + attributes.durabilityKind = DurabilityKind_t::TRANSIENT_LOCAL; + + DOMAIN_LOG("Creating writer[%s, %s]\n", topicName, typeName); + + attributes.reliabilityKind = ReliabilityKind_t::RELIABLE; + + attributes.ownership_Kind = ownership_kind; + attributes.ownership_strenght = ownership_strenght; + + StatefulWriter &writer = m_statefulWriters[m_numStatefulWriters++]; + writer.init(attributes, TopicKind_t::NO_KEY, &m_threadPool, m_transport, + enforceUnicast); + + part.addWriter(&writer); + return &writer; + +} + rtps::Reader *Domain::createReader(Participant &part, const char *topicName, const char *typeName, bool reliable, ip4_addr_t mcastaddress) { From f4faa7abb8f7387ff80938f16afa874b3e582197 Mon Sep 17 00:00:00 2001 From: sven Date: Mon, 27 Sep 2021 16:52:06 +0200 Subject: [PATCH 08/25] announce QoS --- src/discovery/TopicData.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/discovery/TopicData.cpp b/src/discovery/TopicData.cpp index 98b174c7..2c2a0d6b 100644 --- a/src/discovery/TopicData.cpp +++ b/src/discovery/TopicData.cpp @@ -181,6 +181,14 @@ bool TopicData::serializeIntoUcdrBuffer(ucdrBuffer &buffer) const { ucdr_serialize_uint16_t(&buffer, ParameterId::PID_SENTINEL); ucdr_serialize_uint16_t(&buffer, 0); + //Ownership + ucdr_serialize_uint16_t(&buffer, ParameterId::PID_OWNERSHIP); + ucdr_serialize_uint16_t(&buffer, sizeof(OwnershipKind_t)); + ucdr_serialize_uint32_t(&buffer, static_cast(ownership_Kind)); + + ucdr_serialize_uint16_t(&buffer, ParameterId::PID_OWNERSHIP_STRENGTH); + ucdr_serialize_uint16_t(&buffer, sizeof(OwnershipStrength)); + ucdr_serialize_uint32_t(&buffer, ownership_strenght); return true; } From ff96f23e8bf3d7a5676a0f2557fedddc0d775dab Mon Sep 17 00:00:00 2001 From: sven Date: Tue, 28 Sep 2021 23:45:44 +0200 Subject: [PATCH 09/25] matching process doesnt really work Match is not fullfilled on computer side. --- include/rtps/common/types.h | 2 +- include/rtps/discovery/TopicData.h | 2 +- include/rtps/entities/Domain.h | 2 +- include/rtps/messages/MessageFactory.h | 12 ++++++++++-- include/rtps/messages/MessageTypes.h | 8 ++++++++ src/discovery/TopicData.cpp | 27 ++++++++++++++++++++------ src/entities/Domain.cpp | 7 +++++-- 7 files changed, 47 insertions(+), 13 deletions(-) diff --git a/include/rtps/common/types.h b/include/rtps/common/types.h index 61152b26..f1da4780 100644 --- a/include/rtps/common/types.h +++ b/include/rtps/common/types.h @@ -84,7 +84,7 @@ enum class DurabilityKind_t : uint32_t { PERSISTENT = 3 }; -enum class OwnershipKind_t : uint32_t{ +enum class OwnershipKind_t : uint8_t{ SHARED = 0, EXCLUSIVE = 1 }; diff --git a/include/rtps/discovery/TopicData.h b/include/rtps/discovery/TopicData.h index 91799144..2a0263c0 100644 --- a/include/rtps/discovery/TopicData.h +++ b/include/rtps/discovery/TopicData.h @@ -74,7 +74,7 @@ struct TopicData { durabilityKind(DurabilityKind_t::TRANSIENT_LOCAL), unicastLocator(loc) { if(ownership_Kind == OwnershipKind_t::EXCLUSIVE ) { - reliabilityKind(ReliabilityKind_t::RELIABLE); + reliabilityKind = (ReliabilityKind_t::RELIABLE); } }; diff --git a/include/rtps/entities/Domain.h b/include/rtps/entities/Domain.h index ec88833b..1af7607b 100644 --- a/include/rtps/entities/Domain.h +++ b/include/rtps/entities/Domain.h @@ -60,7 +60,7 @@ class Domain { ip4_addr_t mcastaddress = {0}); Reader *createReader(Participant &part, const char *topicName, - const char *typeName, OwnerShipKind_t ownership_Kind, + const char *typeName, OwnershipKind_t ownership_Kind, ip4_addr_t mcastaddress = {0}); Writer *writerExists(Participant &part, const char *topicName, diff --git a/include/rtps/messages/MessageFactory.h b/include/rtps/messages/MessageFactory.h index a4b1865e..e5a54eb7 100644 --- a/include/rtps/messages/MessageFactory.h +++ b/include/rtps/messages/MessageFactory.h @@ -84,7 +84,7 @@ void addSubMessageTimeStamp(Buffer &buffer, bool setInvalid = false) { template void addSubMessageData(Buffer &buffer, const Buffer &filledPayload, bool containsInlineQos, const SequenceNumber_t &SN, - const EntityId_t &writerID, const EntityId_t &readerID) { + const EntityId_t &writerID, const EntityId_t &readerID, const OwnershipKind_t ownershipKind, const OwnershipStrength ownershipStrength) { SubmessageData msg; msg.header.submessageId = SubmessageKind::DATA; #if IS_LITTLE_ENDIAN @@ -112,7 +112,8 @@ void addSubMessageData(Buffer &buffer, const Buffer &filledPayload, constexpr uint16_t octetsToInlineQoS = 4 + 4 + 8; // EntityIds + SequenceNumber msg.octetsToInlineQos = octetsToInlineQoS; - + msg.ownershipKind = ownershipKind; + msg.ownershipStrength = ownershipStrength; serializeMessage(buffer, msg); if (filledPayload.isValid()) { @@ -121,6 +122,13 @@ void addSubMessageData(Buffer &buffer, const Buffer &filledPayload, } } +template +void addSubMessageData(Buffer &buffer, const Buffer &filledPayload, + bool containsInlineQos, const SequenceNumber_t &SN, + const EntityId_t &writerID, const EntityId_t &readerID) { + addSubMessageData(buffer,filledPayload,containsInlineQos,SN,writerID,readerID, OwnershipKind_t::SHARED,0); +} + template void addHeartbeat(Buffer &buffer, EntityId_t writerId, EntityId_t readerId, SequenceNumber_t firstSN, SequenceNumber_t lastSN, diff --git a/include/rtps/messages/MessageTypes.h b/include/rtps/messages/MessageTypes.h index 49d0b5c3..4063ea57 100644 --- a/include/rtps/messages/MessageTypes.h +++ b/include/rtps/messages/MessageTypes.h @@ -175,6 +175,9 @@ struct SubmessageData { EntityId_t readerId; EntityId_t writerId; SequenceNumber_t writerSN; + + OwnershipKind_t ownershipKind; + OwnershipStrength ownershipStrength; static constexpr uint16_t getRawSize() { return SubmessageHeader::getRawSize() + sizeof(uint16_t) + sizeof(uint16_t) + (2 * 3 + 2 * 1) // EntityID @@ -265,6 +268,11 @@ bool serializeMessage(Buffer &buffer, SubmessageData &msg) { sizeof(msg.writerSN.high)); buffer.append(reinterpret_cast(&msg.writerSN.low), sizeof(msg.writerSN.low)); + if(msg.ownershipKind == OwnershipKind_t::EXCLUSIVE){ + buffer.append(reinterpret_cast(SMElement::ParameterId::PID_OWNERSHIP_STRENGTH), sizeof(uint16_t)); + buffer.append(reinterpret_cast(sizeof(OwnershipStrength)), sizeof(OwnershipStrength)); + buffer.append(reinterpret_cast(msg.ownershipStrength), sizeof(OwnershipStrength)); + } return true; } diff --git a/src/discovery/TopicData.cpp b/src/discovery/TopicData.cpp index 2c2a0d6b..29f926a8 100644 --- a/src/discovery/TopicData.cpp +++ b/src/discovery/TopicData.cpp @@ -83,6 +83,19 @@ bool TopicData::readFromUcdrBuffer(ucdrBuffer &buffer) { case ParameterId::PID_MULTICAST_LOCATOR: multicastLocator.readFromUcdrBuffer(buffer); break; + case ParameterId::PID_OWNERSHIP: + uint32_t ownershipKindLength; + ucdr_deserialize_uint32_t(&buffer, &ownershipKindLength); + if(ownershipKindLength == 1) { + ucdr_deserialize_uint8_t(&buffer, reinterpret_cast(&ownership_Kind)); + } + break; + case ParameterId::PID_OWNERSHIP_STRENGTH: + uint32_t ownershipStrenghtLength; + ucdr_deserialize_uint32_t(&buffer, &ownershipStrenghtLength); + if(ownershipStrenghtLength == 4) + ucdr_deserialize_uint32_t(&buffer, &ownership_strenght); + break; default: buffer.iterator += length; buffer.last_data_size = 1; @@ -169,9 +182,9 @@ bool TopicData::serializeIntoUcdrBuffer(ucdrBuffer &buffer) const { const uint8_t unidentifiedOffset = 8; ucdr_serialize_uint16_t(&buffer, ParameterId::PID_RELIABILITY); - ucdr_serialize_uint16_t(&buffer, - sizeof(ReliabilityKind_t) + unidentifiedOffset); + ucdr_serialize_uint16_t(&buffer,sizeof(ReliabilityKind_t) + unidentifiedOffset); ucdr_serialize_uint32_t(&buffer, static_cast(reliabilityKind)); + ucdr_serialize_uint32_t(&buffer, 0); // unidentified additional value ucdr_serialize_uint32_t(&buffer, 0); // unidentified additional value @@ -184,11 +197,13 @@ bool TopicData::serializeIntoUcdrBuffer(ucdrBuffer &buffer) const { //Ownership ucdr_serialize_uint16_t(&buffer, ParameterId::PID_OWNERSHIP); ucdr_serialize_uint16_t(&buffer, sizeof(OwnershipKind_t)); - ucdr_serialize_uint32_t(&buffer, static_cast(ownership_Kind)); + ucdr_serialize_uint8_t(&buffer, static_cast(ownership_Kind)); - ucdr_serialize_uint16_t(&buffer, ParameterId::PID_OWNERSHIP_STRENGTH); - ucdr_serialize_uint16_t(&buffer, sizeof(OwnershipStrength)); - ucdr_serialize_uint32_t(&buffer, ownership_strenght); + if(ownership_Kind == OwnershipKind_t::EXCLUSIVE) { + ucdr_serialize_uint16_t(&buffer, ParameterId::PID_OWNERSHIP_STRENGTH); + ucdr_serialize_uint16_t(&buffer, sizeof(OwnershipStrength)); + ucdr_serialize_uint32_t(&buffer, ownership_strenght); + } return true; } diff --git a/src/entities/Domain.cpp b/src/entities/Domain.cpp index 7f97f2bd..2959ab38 100644 --- a/src/entities/Domain.cpp +++ b/src/entities/Domain.cpp @@ -336,6 +336,7 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, attributes.unicastLocator = getUserUnicastLocator(part.m_participantId); attributes.durabilityKind = DurabilityKind_t::TRANSIENT_LOCAL; + attributes.ownership_Kind = OwnershipKind_t::SHARED; DOMAIN_LOG("Creating writer[%s, %s]\n", topicName, typeName); if (reliable) { @@ -363,11 +364,13 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, const char *typeName, OwnershipKind_t ownership_kind, OwnershipStrength ownership_strenght, bool enforceUnicast){ // Check if there is enough capacity for more writers Ownership currently just support reliable writer so its compatible with fastDDS (dont really get why thats a problem) - if (m_statefulWriters.size() <= m_numStatefulWriters) { + if (m_statefulWriters.size() <= m_numStatefulWriters && (ownership_kind == OwnershipKind_t::EXCLUSIVE)) { DOMAIN_LOG("No Writer created. Max Number of Writers reached.\n"); return nullptr; } - + if(OwnershipKind_t::SHARED == ownership_kind) { + return createWriter(part, topicName, typeName, false, enforceUnicast); + } // TODO Distinguish WithKey and NoKey (Also changes EntityKind) TopicData attributes; From 10a2846039a38595f24da97d72170ae3de147c2d Mon Sep 17 00:00:00 2001 From: sven Date: Sat, 2 Oct 2021 13:43:48 +0200 Subject: [PATCH 10/25] Some changes --- include/rtps/discovery/TopicData.h | 5 ++++- include/rtps/messages/MessageTypes.h | 3 +++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/include/rtps/discovery/TopicData.h b/include/rtps/discovery/TopicData.h index 2a0263c0..8d5f9d53 100644 --- a/include/rtps/discovery/TopicData.h +++ b/include/rtps/discovery/TopicData.h @@ -48,7 +48,7 @@ struct TopicData { Locator multicastLocator; OwnershipKind_t ownership_Kind; OwnershipStrength ownership_strenght; - + TopicKind_t topicKind; TopicData() : endpointGuid(GUID_UNKNOWN), typeName{'\0'}, topicName{'\0'}, reliabilityKind(ReliabilityKind_t::BEST_EFFORT), @@ -59,6 +59,7 @@ struct TopicData { multicastLocator = Locator(); ownership_Kind = OwnershipKind_t::SHARED; ownership_strenght = 0; + topicKind = TopicKind_t::NO_KEY; }; TopicData(Guid_t guid, ReliabilityKind_t reliability, Locator loc) @@ -66,6 +67,7 @@ struct TopicData { reliabilityKind(reliability), durabilityKind(DurabilityKind_t::TRANSIENT_LOCAL), unicastLocator(loc) , ownership_Kind(OwnershipKind_t::SHARED), ownership_strenght(0){ + topicKind = TopicKind_t::NO_KEY; }; TopicData(Guid_t guid, OwnershipKind_t kind, OwnershipStrength strength, Locator loc) @@ -75,6 +77,7 @@ struct TopicData { if(ownership_Kind == OwnershipKind_t::EXCLUSIVE ) { reliabilityKind = (ReliabilityKind_t::RELIABLE); + topicKind = TopicKind_t::WITH_KEY; } }; diff --git a/include/rtps/messages/MessageTypes.h b/include/rtps/messages/MessageTypes.h index 4063ea57..8dd55b19 100644 --- a/include/rtps/messages/MessageTypes.h +++ b/include/rtps/messages/MessageTypes.h @@ -272,6 +272,9 @@ bool serializeMessage(Buffer &buffer, SubmessageData &msg) { buffer.append(reinterpret_cast(SMElement::ParameterId::PID_OWNERSHIP_STRENGTH), sizeof(uint16_t)); buffer.append(reinterpret_cast(sizeof(OwnershipStrength)), sizeof(OwnershipStrength)); buffer.append(reinterpret_cast(msg.ownershipStrength), sizeof(OwnershipStrength)); + buffer.append(reinterpret_cast(SMElement::ParameterId::PID_KEY_HASH), sizeof(uint16_t)); + uint16_t key = 1; + buffer.append(reinterpret_cast(key), sizeof(uint16_t)); } return true; } From 85a3f688a0d857b41f8bbb8e6b34c590b265ecde Mon Sep 17 00:00:00 2001 From: sven Date: Mon, 4 Oct 2021 00:50:13 +0200 Subject: [PATCH 11/25] clean up --- include/rtps/discovery/TopicData.h | 1 + src/discovery/TopicData.cpp | 1 + src/entities/Domain.cpp | 7 +++++-- 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/include/rtps/discovery/TopicData.h b/include/rtps/discovery/TopicData.h index 8d5f9d53..7e068cba 100644 --- a/include/rtps/discovery/TopicData.h +++ b/include/rtps/discovery/TopicData.h @@ -78,6 +78,7 @@ struct TopicData { if(ownership_Kind == OwnershipKind_t::EXCLUSIVE ) { reliabilityKind = (ReliabilityKind_t::RELIABLE); topicKind = TopicKind_t::WITH_KEY; + endpointGuid.entityId.entityKind = EntityKind_t::USER_DEFINED_WRITER_WITH_KEY; } }; diff --git a/src/discovery/TopicData.cpp b/src/discovery/TopicData.cpp index 29f926a8..9caf2b00 100644 --- a/src/discovery/TopicData.cpp +++ b/src/discovery/TopicData.cpp @@ -164,6 +164,7 @@ bool TopicData::serializeIntoUcdrBuffer(ucdrBuffer &buffer) const { ucdr_serialize_uint16_t(&buffer, ParameterId::PID_KEY_HASH); ucdr_serialize_uint16_t(&buffer, guidSize); + ucdr_serialize_array_uint8_t(&buffer, endpointGuid.prefix.id.data(), endpointGuid.prefix.id.size()); ucdr_serialize_array_uint8_t(&buffer, endpointGuid.entityId.entityKey.data(), diff --git a/src/entities/Domain.cpp b/src/entities/Domain.cpp index 2959ab38..646ca2cf 100644 --- a/src/entities/Domain.cpp +++ b/src/entities/Domain.cpp @@ -393,9 +393,12 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, attributes.ownership_Kind = ownership_kind; attributes.ownership_strenght = ownership_strenght; - + attributes.topicKind = TopicKind_t::WITH_KEY; + if(attributes.topicKind == TopicKind_t::WITH_KEY){ + attributes.endpointGuid.entityId.entityKind = EntityKind_t::USER_DEFINED_WRITER_WITH_KEY; + } StatefulWriter &writer = m_statefulWriters[m_numStatefulWriters++]; - writer.init(attributes, TopicKind_t::NO_KEY, &m_threadPool, m_transport, + writer.init(attributes, attributes.topicKind, &m_threadPool, m_transport, enforceUnicast); part.addWriter(&writer); From a9a83597f186fa511b81df00c586fa7cf0d172f5 Mon Sep 17 00:00:00 2001 From: sven Date: Mon, 4 Oct 2021 22:50:15 +0200 Subject: [PATCH 12/25] Increased Pbuf size , Fast dds sends to much stuff --- include/rtps/discovery/SEDPAgent.h | 2 +- include/rtps/discovery/TopicData.h | 30 +-- include/rtps/messages/MessageFactory.h | 240 ++++++++++---------- include/rtps/messages/MessageTypes.h | 4 +- src/ThreadPool.cpp | 2 +- src/discovery/SEDPAgent.cpp | 301 ++++++++++++------------- src/discovery/TopicData.cpp | 17 +- src/entities/Domain.cpp | 48 +--- 8 files changed, 293 insertions(+), 351 deletions(-) diff --git a/include/rtps/discovery/SEDPAgent.h b/include/rtps/discovery/SEDPAgent.h index 0f2b9697..014e41b2 100644 --- a/include/rtps/discovery/SEDPAgent.h +++ b/include/rtps/discovery/SEDPAgent.h @@ -52,7 +52,7 @@ class SEDPAgent { private: Participant *m_part; sys_mutex_t m_mutex; - uint8_t m_buffer[600]; // TODO check size, currently changed from 300 to 600 + uint8_t m_buffer[900]; // TODO check size, currently changed from 300 to 600 // (FastDDS gives too much options) BuiltInEndpoints m_endpoints; void (*mfp_onNewPublisherCallback)(void *arg) = nullptr; diff --git a/include/rtps/discovery/TopicData.h b/include/rtps/discovery/TopicData.h index 7e068cba..13502b4d 100644 --- a/include/rtps/discovery/TopicData.h +++ b/include/rtps/discovery/TopicData.h @@ -46,9 +46,9 @@ struct TopicData { DurabilityKind_t durabilityKind; Locator unicastLocator; Locator multicastLocator; - OwnershipKind_t ownership_Kind; - OwnershipStrength ownership_strenght; - TopicKind_t topicKind; + //OwnershipKind_t ownership_Kind; + //OwnershipStrength ownership_strenght; + //TopicKind_t topicKind; TopicData() : endpointGuid(GUID_UNKNOWN), typeName{'\0'}, topicName{'\0'}, reliabilityKind(ReliabilityKind_t::BEST_EFFORT), @@ -57,29 +57,29 @@ struct TopicData { 192, 168, 0, 42, rtps::getUserUnicastPort(0)); unicastLocator = someLocator; multicastLocator = Locator(); - ownership_Kind = OwnershipKind_t::SHARED; - ownership_strenght = 0; - topicKind = TopicKind_t::NO_KEY; + //ownership_Kind = OwnershipKind_t::SHARED; + // ownership_strenght = 0; + // topicKind = TopicKind_t::NO_KEY; }; TopicData(Guid_t guid, ReliabilityKind_t reliability, Locator loc) : endpointGuid(guid), typeName{'\0'}, topicName{'\0'}, reliabilityKind(reliability), - durabilityKind(DurabilityKind_t::TRANSIENT_LOCAL), unicastLocator(loc) , - ownership_Kind(OwnershipKind_t::SHARED), ownership_strenght(0){ - topicKind = TopicKind_t::NO_KEY; + durabilityKind(DurabilityKind_t::TRANSIENT_LOCAL), unicastLocator(loc) { + // ownership_Kind(OwnershipKind_t::SHARED), ownership_strenght(0){ + //topicKind = TopicKind_t::NO_KEY; }; TopicData(Guid_t guid, OwnershipKind_t kind, OwnershipStrength strength, Locator loc) : endpointGuid(guid), typeName{'\0'}, topicName{'\0'}, - ownership_Kind(kind), ownership_strenght(strength), + // ownership_Kind(kind), ownership_strenght(strength), durabilityKind(DurabilityKind_t::TRANSIENT_LOCAL), unicastLocator(loc) { - if(ownership_Kind == OwnershipKind_t::EXCLUSIVE ) { - reliabilityKind = (ReliabilityKind_t::RELIABLE); - topicKind = TopicKind_t::WITH_KEY; - endpointGuid.entityId.entityKind = EntityKind_t::USER_DEFINED_WRITER_WITH_KEY; - } + //if(ownership_Kind == OwnershipKind_t::EXCLUSIVE ) { + // reliabilityKind = (ReliabilityKind_t::RELIABLE); + // topicKind = TopicKind_t::WITH_KEY; + // endpointGuid.entityId.entityKind = EntityKind_t::USER_DEFINED_WRITER_WITH_KEY; + //} }; bool matchesTopicOf(const TopicData &other); diff --git a/include/rtps/messages/MessageFactory.h b/include/rtps/messages/MessageFactory.h index e5a54eb7..87275e39 100644 --- a/include/rtps/messages/MessageFactory.h +++ b/include/rtps/messages/MessageFactory.h @@ -16,9 +16,7 @@ AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE - This file is part of embeddedRTPS. - Author: i11 - Embedded Software, RWTH Aachen University */ @@ -34,147 +32,139 @@ Author: i11 - Embedded Software, RWTH Aachen University #include namespace rtps { -namespace MessageFactory { -const std::array PROTOCOL_TYPE{'R', 'T', 'P', 'S'}; -const uint8_t numBytesUntilEndOfLength = - 4; // The first bytes incl. submessagelength don't count + namespace MessageFactory { + const std::array PROTOCOL_TYPE{'R', 'T', 'P', 'S'}; + const uint8_t numBytesUntilEndOfLength = + 4; // The first bytes incl. submessagelength don't count -template -void addHeader(Buffer &buffer, const GuidPrefix_t &guidPrefix) { + template + void addHeader(Buffer &buffer, const GuidPrefix_t &guidPrefix) { - Header header; - header.protocolName = PROTOCOL_TYPE; - header.protocolVersion = PROTOCOLVERSION; - header.vendorId = Config::VENDOR_ID; - header.guidPrefix = guidPrefix; + Header header; + header.protocolName = PROTOCOL_TYPE; + header.protocolVersion = PROTOCOLVERSION; + header.vendorId = Config::VENDOR_ID; + header.guidPrefix = guidPrefix; - serializeMessage(buffer, header); -} + serializeMessage(buffer, header); + } -template -void addSubMessageTimeStamp(Buffer &buffer, bool setInvalid = false) { - SubmessageHeader header; - header.submessageId = SubmessageKind::INFO_TS; + template + void addSubMessageTimeStamp(Buffer &buffer, bool setInvalid = false) { + SubmessageHeader header; + header.submessageId = SubmessageKind::INFO_TS; #if IS_LITTLE_ENDIAN - header.flags = FLAG_LITTLE_ENDIAN; + header.flags = FLAG_LITTLE_ENDIAN; #else - header.flags = FLAG_BIG_ENDIAN; + header.flags = FLAG_BIG_ENDIAN; #endif - if (setInvalid) { - header.flags |= FLAG_INVALIDATE; - header.octetsToNextHeader = 0; - } else { - header.octetsToNextHeader = sizeof(Time_t); - } - - serializeMessage(buffer, header); - - if (!setInvalid) { - buffer.reserve(header.octetsToNextHeader); - Time_t now = getCurrentTimeStamp(); - buffer.append(reinterpret_cast(&now.seconds), - sizeof(Time_t::seconds)); - buffer.append(reinterpret_cast(&now.fraction), - sizeof(Time_t::fraction)); - } -} - -template -void addSubMessageData(Buffer &buffer, const Buffer &filledPayload, - bool containsInlineQos, const SequenceNumber_t &SN, - const EntityId_t &writerID, const EntityId_t &readerID, const OwnershipKind_t ownershipKind, const OwnershipStrength ownershipStrength) { - SubmessageData msg; - msg.header.submessageId = SubmessageKind::DATA; + if (setInvalid) { + header.flags |= FLAG_INVALIDATE; + header.octetsToNextHeader = 0; + } else { + header.octetsToNextHeader = sizeof(Time_t); + } + + serializeMessage(buffer, header); + + if (!setInvalid) { + buffer.reserve(header.octetsToNextHeader); + Time_t now = getCurrentTimeStamp(); + buffer.append(reinterpret_cast(&now.seconds), + sizeof(Time_t::seconds)); + buffer.append(reinterpret_cast(&now.fraction), + sizeof(Time_t::fraction)); + } + } + + template + void addSubMessageData(Buffer &buffer, const Buffer &filledPayload, + bool containsInlineQos, const SequenceNumber_t &SN, + const EntityId_t &writerID, const EntityId_t &readerID) { + SubmessageData msg; + msg.header.submessageId = SubmessageKind::DATA; #if IS_LITTLE_ENDIAN - msg.header.flags = FLAG_LITTLE_ENDIAN; + msg.header.flags = FLAG_LITTLE_ENDIAN; #else - msg.header.flags = FLAG_BIG_ENDIAN; + msg.header.flags = FLAG_BIG_ENDIAN; #endif - msg.header.octetsToNextHeader = SubmessageData::getRawSize() + - filledPayload.spaceUsed() - - numBytesUntilEndOfLength; - - if (containsInlineQos) { - msg.header.flags |= FLAG_INLINE_QOS; - } - if (filledPayload.isValid()) { - msg.header.flags |= FLAG_DATA_PAYLOAD; - } - - msg.writerSN = SN; - msg.extraFlags = 0; - msg.readerId = readerID; - msg.writerId = writerID; - - constexpr uint16_t octetsToInlineQoS = - 4 + 4 + 8; // EntityIds + SequenceNumber - msg.octetsToInlineQos = octetsToInlineQoS; - msg.ownershipKind = ownershipKind; - msg.ownershipStrength = ownershipStrength; - serializeMessage(buffer, msg); - - if (filledPayload.isValid()) { - Buffer shallowCopy = filledPayload; - buffer.append(std::move(shallowCopy)); - } -} - -template -void addSubMessageData(Buffer &buffer, const Buffer &filledPayload, - bool containsInlineQos, const SequenceNumber_t &SN, - const EntityId_t &writerID, const EntityId_t &readerID) { - addSubMessageData(buffer,filledPayload,containsInlineQos,SN,writerID,readerID, OwnershipKind_t::SHARED,0); -} - -template -void addHeartbeat(Buffer &buffer, EntityId_t writerId, EntityId_t readerId, - SequenceNumber_t firstSN, SequenceNumber_t lastSN, - Count_t count) { - SubmessageHeartbeat subMsg; - subMsg.header.submessageId = SubmessageKind::HEARTBEAT; - subMsg.header.octetsToNextHeader = - SubmessageHeartbeat::getRawSize() - numBytesUntilEndOfLength; + msg.header.octetsToNextHeader = SubmessageData::getRawSize() + + filledPayload.spaceUsed() - + numBytesUntilEndOfLength; + + if (containsInlineQos) { + msg.header.flags |= FLAG_INLINE_QOS; + } + if (filledPayload.isValid()) { + msg.header.flags |= FLAG_DATA_PAYLOAD; + } + + msg.writerSN = SN; + msg.extraFlags = 0; + msg.readerId = readerID; + msg.writerId = writerID; + + constexpr uint16_t octetsToInlineQoS = + 4 + 4 + 8; // EntityIds + SequenceNumber + msg.octetsToInlineQos = octetsToInlineQoS; + + serializeMessage(buffer, msg); + + if (filledPayload.isValid()) { + Buffer shallowCopy = filledPayload; + buffer.append(std::move(shallowCopy)); + } + } + + template + void addHeartbeat(Buffer &buffer, EntityId_t writerId, EntityId_t readerId, + SequenceNumber_t firstSN, SequenceNumber_t lastSN, + Count_t count) { + SubmessageHeartbeat subMsg; + subMsg.header.submessageId = SubmessageKind::HEARTBEAT; + subMsg.header.octetsToNextHeader = + SubmessageHeartbeat::getRawSize() - numBytesUntilEndOfLength; #if IS_LITTLE_ENDIAN - subMsg.header.flags = FLAG_LITTLE_ENDIAN; + subMsg.header.flags = FLAG_LITTLE_ENDIAN; #else - subMsg.header.flags = FLAG_BIG_ENDIAN; + subMsg.header.flags = FLAG_BIG_ENDIAN; #endif - // Force response by not setting final flag. - - subMsg.writerId = writerId; - subMsg.readerId = readerId; - subMsg.firstSN = firstSN; - subMsg.lastSN = lastSN; - subMsg.count = count; - - serializeMessage(buffer, subMsg); -} - -template -void addAckNack(Buffer &buffer, EntityId_t writerId, EntityId_t readerId, - SequenceNumberSet readerSNState, Count_t count) { - SubmessageAckNack subMsg; - subMsg.header.submessageId = SubmessageKind::ACKNACK; + // Force response by not setting final flag. + + subMsg.writerId = writerId; + subMsg.readerId = readerId; + subMsg.firstSN = firstSN; + subMsg.lastSN = lastSN; + subMsg.count = count; + + serializeMessage(buffer, subMsg); + } + + template + void addAckNack(Buffer &buffer, EntityId_t writerId, EntityId_t readerId, + SequenceNumberSet readerSNState, Count_t count) { + SubmessageAckNack subMsg; + subMsg.header.submessageId = SubmessageKind::ACKNACK; #if IS_LITTLE_ENDIAN - subMsg.header.flags = FLAG_LITTLE_ENDIAN; + subMsg.header.flags = FLAG_LITTLE_ENDIAN; #else - subMsg.header.flags = FLAG_BIG_ENDIAN; + subMsg.header.flags = FLAG_BIG_ENDIAN; #endif - subMsg.header.flags |= FLAG_FINAL; // For now, we don't want any response - subMsg.header.octetsToNextHeader = - SubmessageAckNack::getRawSize(readerSNState) - numBytesUntilEndOfLength; - - subMsg.writerId = writerId; - subMsg.readerId = readerId; - subMsg.readerSNState = readerSNState; - subMsg.count = count; - - serializeMessage(buffer, subMsg); -} -} // namespace MessageFactory + subMsg.header.flags |= FLAG_FINAL; // For now, we don't want any response + subMsg.header.octetsToNextHeader = + SubmessageAckNack::getRawSize(readerSNState) - numBytesUntilEndOfLength; + + subMsg.writerId = writerId; + subMsg.readerId = readerId; + subMsg.readerSNState = readerSNState; + subMsg.count = count; + + serializeMessage(buffer, subMsg); + } + } // namespace MessageFactory } // namespace rtps -#endif // RTPS_MESSAGEFACTORY_H +#endif // RTPS_MESSAGEFACTORY_H \ No newline at end of file diff --git a/include/rtps/messages/MessageTypes.h b/include/rtps/messages/MessageTypes.h index 8dd55b19..2bfe01f0 100644 --- a/include/rtps/messages/MessageTypes.h +++ b/include/rtps/messages/MessageTypes.h @@ -268,14 +268,14 @@ bool serializeMessage(Buffer &buffer, SubmessageData &msg) { sizeof(msg.writerSN.high)); buffer.append(reinterpret_cast(&msg.writerSN.low), sizeof(msg.writerSN.low)); - if(msg.ownershipKind == OwnershipKind_t::EXCLUSIVE){ + /*if(msg.ownershipKind == OwnershipKind_t::EXCLUSIVE){ buffer.append(reinterpret_cast(SMElement::ParameterId::PID_OWNERSHIP_STRENGTH), sizeof(uint16_t)); buffer.append(reinterpret_cast(sizeof(OwnershipStrength)), sizeof(OwnershipStrength)); buffer.append(reinterpret_cast(msg.ownershipStrength), sizeof(OwnershipStrength)); buffer.append(reinterpret_cast(SMElement::ParameterId::PID_KEY_HASH), sizeof(uint16_t)); uint16_t key = 1; buffer.append(reinterpret_cast(key), sizeof(uint16_t)); - } + }*/ return true; } diff --git a/src/ThreadPool.cpp b/src/ThreadPool.cpp index 5390ce11..6f6b5b75 100644 --- a/src/ThreadPool.cpp +++ b/src/ThreadPool.cpp @@ -204,7 +204,7 @@ void ThreadPool::doReaderWork() { continue; } - m_receiveJumppad(m_callee, const_cast(packet)); + m_receiveJumppad(m_callee, const_cast(packet)); } } diff --git a/src/discovery/SEDPAgent.cpp b/src/discovery/SEDPAgent.cpp index cb5f98d2..99a3ded4 100644 --- a/src/discovery/SEDPAgent.cpp +++ b/src/discovery/SEDPAgent.cpp @@ -16,9 +16,7 @@ AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE - This file is part of embeddedRTPS. - Author: i11 - Embedded Software, RWTH Aachen University */ @@ -45,220 +43,221 @@ using rtps::SEDPAgent; #endif void SEDPAgent::init(Participant &part, const BuiltInEndpoints &endpoints) { - // TODO move - if (sys_mutex_new(&m_mutex) != ERR_OK) { - SEDP_LOG("SEDPAgent failed to create mutex\n"); - return; - } + // TODO move + if (sys_mutex_new(&m_mutex) != ERR_OK) { + SEDP_LOG("SEDPAgent failed to create mutex\n"); + return; + } - m_part = ∂ - m_endpoints = endpoints; - if (m_endpoints.sedpPubReader != nullptr) { - m_endpoints.sedpPubReader->registerCallback(receiveCallbackPublisher, this); - } - if (m_endpoints.sedpSubReader != nullptr) { - m_endpoints.sedpSubReader->registerCallback(receiveCallbackSubscriber, - this); - } + m_part = ∂ + m_endpoints = endpoints; + if (m_endpoints.sedpPubReader != nullptr) { + m_endpoints.sedpPubReader->registerCallback(receiveCallbackPublisher, this); + } + if (m_endpoints.sedpSubReader != nullptr) { + m_endpoints.sedpSubReader->registerCallback(receiveCallbackSubscriber, + this); + } } void SEDPAgent::registerOnNewPublisherMatchedCallback( - void (*callback)(void *arg), void *args) { - mfp_onNewPublisherCallback = callback; - m_onNewPublisherArgs = args; + void (*callback)(void *arg), void *args) { + mfp_onNewPublisherCallback = callback; + m_onNewPublisherArgs = args; } void SEDPAgent::registerOnNewSubscriberMatchedCallback( - void (*callback)(void *arg), void *args) { - mfp_onNewSubscriberCallback = callback; - m_onNewSubscriberArgs = args; + void (*callback)(void *arg), void *args) { + mfp_onNewSubscriberCallback = callback; + m_onNewSubscriberArgs = args; } void SEDPAgent::receiveCallbackPublisher(void *callee, const ReaderCacheChange &cacheChange) { - auto agent = static_cast(callee); - agent->onNewPublisher(cacheChange); + auto agent = static_cast(callee); + agent->onNewPublisher(cacheChange); } void SEDPAgent::receiveCallbackSubscriber( - void *callee, const ReaderCacheChange &cacheChange) { - auto agent = static_cast(callee); - agent->onNewSubscriber(cacheChange); + void *callee, const ReaderCacheChange &cacheChange) { + auto agent = static_cast(callee); + agent->onNewSubscriber(cacheChange); } void SEDPAgent::onNewPublisher(const ReaderCacheChange &change) { - Lock lock{m_mutex}; + Lock lock{m_mutex}; #if SEDP_VERBOSE - SEDP_LOG("New publisher\n"); + SEDP_LOG("New publisher\n"); #endif - if (!change.copyInto(m_buffer, sizeof(m_buffer) / sizeof(m_buffer[0]))) { + if (!change.copyInto(m_buffer, sizeof(m_buffer) / sizeof(m_buffer[0]))) { #if SEDP_VERBOSE - SEDP_LOG("EDPAgent: Buffer too small.\n"); + SEDP_LOG("EDPAgent: Buffer too small.\n"); #endif - return; - } - ucdrBuffer cdrBuffer; - ucdr_init_buffer(&cdrBuffer, m_buffer, sizeof(m_buffer)); + return; + } + ucdrBuffer cdrBuffer; + ucdr_init_buffer(&cdrBuffer, m_buffer, sizeof(m_buffer)); - TopicData topicData; - if (topicData.readFromUcdrBuffer(cdrBuffer)) { - onNewPublisher(topicData); - } + TopicData topicData; + if (topicData.readFromUcdrBuffer(cdrBuffer)) { + onNewPublisher(topicData); + } } void SEDPAgent::onNewPublisher(const TopicData &writerData) { - // TODO Is it okay to add Endpoint if the respective participant is unknown - // participant? - if (!m_part->findRemoteParticipant(writerData.endpointGuid.prefix)) { - return; - } + // TODO Is it okay to add Endpoint if the respective participant is unknown + // participant? + if (!m_part->findRemoteParticipant(writerData.endpointGuid.prefix)) { + return; + } #if SEDP_VERBOSE - SEDP_LOG("PUB T/D %s/%s", writerData.topicName, writerData.typeName); + SEDP_LOG("PUB T/D %s/%s", writerData.topicName, writerData.typeName); #endif - Reader *reader = m_part->getMatchingReader(writerData); - if (reader == nullptr) { + Reader *reader = m_part->getMatchingReader(writerData); + if (reader == nullptr) { #if SEDP_VERBOSE - SEDP_LOG("SEDPAgent: Couldn't find reader for new Publisher[%s, %s]\n", - writerData.topicName, writerData.typeName); + SEDP_LOG("SEDPAgent: Couldn't find reader for new Publisher[%s, %s]\n", + writerData.topicName, writerData.typeName); #endif - return; - } - // TODO check policies + return; + } + // TODO check policies #if SEDP_VERBOSE - SEDP_LOG("Found a new "); - if (writerData.reliabilityKind == ReliabilityKind_t::RELIABLE) { - SEDP_LOG("reliable "); - } else { - SEDP_LOG("best-effort "); - } - SEDP_LOG("publisher\n"); + SEDP_LOG("Found a new "); + if (writerData.reliabilityKind == ReliabilityKind_t::RELIABLE) { + SEDP_LOG("reliable "); + } else { + SEDP_LOG("best-effort "); + } + SEDP_LOG("publisher\n"); #endif - reader->addNewMatchedWriter( - WriterProxy{writerData.endpointGuid, writerData.unicastLocator}); - if (mfp_onNewPublisherCallback != nullptr) { - mfp_onNewPublisherCallback(m_onNewPublisherArgs); - } + reader->addNewMatchedWriter( + WriterProxy{writerData.endpointGuid, writerData.unicastLocator}); + if (mfp_onNewPublisherCallback != nullptr) { + mfp_onNewPublisherCallback(m_onNewPublisherArgs); + } } void SEDPAgent::onNewSubscriber(const ReaderCacheChange &change) { - Lock lock{m_mutex}; + Lock lock{m_mutex}; #if SEDP_VERBOSE - SEDP_LOG("New subscriber\n"); + SEDP_LOG("New subscriber\n"); #endif - if (!change.copyInto(m_buffer, sizeof(m_buffer) / sizeof(m_buffer[0]))) { + if (!change.copyInto(m_buffer, sizeof(m_buffer) / sizeof(m_buffer[0]))) { #if SEDP_VERBOSE - SEDP_LOG("SEDPAgent: Buffer too small."); + SEDP_LOG("SEDPAgent: Buffer too small."); #endif - return; - } - ucdrBuffer cdrBuffer; - ucdr_init_buffer(&cdrBuffer, m_buffer, sizeof(m_buffer)); + return; + } + ucdrBuffer cdrBuffer; + ucdr_init_buffer(&cdrBuffer, m_buffer, sizeof(m_buffer)); - TopicData topicData; - if (topicData.readFromUcdrBuffer(cdrBuffer)) { - onNewSubscriber(topicData); - } + TopicData topicData; + if (topicData.readFromUcdrBuffer(cdrBuffer)) { + onNewSubscriber(topicData); + } } void SEDPAgent::onNewSubscriber(const TopicData &readerData) { - if (!m_part->findRemoteParticipant(readerData.endpointGuid.prefix)) { - return; - } - Writer *writer = m_part->getMatchingWriter(readerData); + if (!m_part->findRemoteParticipant(readerData.endpointGuid.prefix)) { + return; + } + Writer *writer = m_part->getMatchingWriter(readerData); #if SEDP_VERBOSE - SEDP_LOG("SUB T/D %s/%s", readerData.topicName, readerData.typeName); + SEDP_LOG("SUB T/D %s/%s", readerData.topicName, readerData.typeName); #endif - if (writer == nullptr) { + if (writer == nullptr) { #if SEDP_VERBOSE - SEDP_LOG("SEDPAgent: Couldn't find writer for new subscriber[%s, %s]\n", - readerData.topicName, readerData.typeName); + SEDP_LOG("SEDPAgent: Couldn't find writer for new subscriber[%s, %s]\n", + readerData.topicName, readerData.typeName); #endif - return; - } + return; + } - // TODO check policies + // TODO check policies #if SEDP_VERBOSE - SEDP_LOG("Found a new "); - if (readerData.reliabilityKind == ReliabilityKind_t::RELIABLE) { - SEDP_LOG("reliable "); - } else { - SEDP_LOG("best-effort "); - } - SEDP_LOG("Subscriber\n"); + SEDP_LOG("Found a new "); + if (readerData.reliabilityKind == ReliabilityKind_t::RELIABLE) { + SEDP_LOG("reliable "); + } else { + SEDP_LOG("best-effort "); + } + SEDP_LOG("Subscriber\n"); #endif - if (readerData.multicastLocator.kind == - rtps::LocatorKind_t::LOCATOR_KIND_UDPv4) { - writer->addNewMatchedReader(ReaderProxy{readerData.endpointGuid, - readerData.unicastLocator, - readerData.multicastLocator}); - } else { - writer->addNewMatchedReader( - ReaderProxy{readerData.endpointGuid, readerData.unicastLocator}); - } + if (readerData.multicastLocator.kind == + rtps::LocatorKind_t::LOCATOR_KIND_UDPv4) { + writer->addNewMatchedReader(ReaderProxy{readerData.endpointGuid, + readerData.unicastLocator, + readerData.multicastLocator}); + } else { + writer->addNewMatchedReader( + ReaderProxy{readerData.endpointGuid, readerData.unicastLocator}); + } - if (mfp_onNewSubscriberCallback != nullptr) { - mfp_onNewSubscriberCallback(m_onNewSubscriberArgs); - } + if (mfp_onNewSubscriberCallback != nullptr) { + mfp_onNewSubscriberCallback(m_onNewSubscriberArgs); + } } void SEDPAgent::addWriter(Writer &writer) { - if (m_endpoints.sedpPubWriter == nullptr) { - return; - } - EntityKind_t writerKind = - writer.m_attributes.endpointGuid.entityId.entityKind; - if (writerKind == EntityKind_t::BUILD_IN_WRITER_WITH_KEY || - writerKind == EntityKind_t::BUILD_IN_WRITER_WITHOUT_KEY) { - return; // No need to announce builtin endpoints - } + if (m_endpoints.sedpPubWriter == nullptr) { + return; + } + EntityKind_t writerKind = + writer.m_attributes.endpointGuid.entityId.entityKind; + if (writerKind == EntityKind_t::BUILD_IN_WRITER_WITH_KEY || + writerKind == EntityKind_t::BUILD_IN_WRITER_WITHOUT_KEY) { + return; // No need to announce builtin endpoints + } - Lock lock{m_mutex}; - ucdrBuffer microbuffer; - ucdr_init_buffer(µbuffer, m_buffer, - sizeof(m_buffer) / sizeof(m_buffer[0])); - const uint16_t zero_options = 0; + Lock lock{m_mutex}; + ucdrBuffer microbuffer; + memset(m_buffer,0, sizeof(m_buffer)); + ucdr_init_buffer(µbuffer, m_buffer, + sizeof(m_buffer) / sizeof(m_buffer[0])); + const uint16_t zero_options = 0; - ucdr_serialize_array_uint8_t(µbuffer, - rtps::SMElement::SCHEME_PL_CDR_LE.data(), - rtps::SMElement::SCHEME_PL_CDR_LE.size()); - ucdr_serialize_uint16_t(µbuffer, zero_options); - writer.m_attributes.serializeIntoUcdrBuffer(microbuffer); - m_endpoints.sedpPubWriter->newChange(ChangeKind_t::ALIVE, m_buffer, - ucdr_buffer_length(µbuffer)); + ucdr_serialize_array_uint8_t(µbuffer, + rtps::SMElement::SCHEME_PL_CDR_LE.data(), + rtps::SMElement::SCHEME_PL_CDR_LE.size()); + ucdr_serialize_uint16_t(µbuffer, zero_options); + writer.m_attributes.serializeIntoUcdrBuffer(microbuffer); + m_endpoints.sedpPubWriter->newChange(ChangeKind_t::ALIVE, m_buffer, + ucdr_buffer_length(µbuffer)); #if SEDP_VERBOSE - SEDP_LOG("Added new change to sedpPubWriter.\n"); + SEDP_LOG("Added new change to sedpPubWriter.\n"); #endif } void SEDPAgent::addReader(Reader &reader) { - if (m_endpoints.sedpSubWriter == nullptr) { - return; - } + if (m_endpoints.sedpSubWriter == nullptr) { + return; + } - EntityKind_t readerKind = - reader.m_attributes.endpointGuid.entityId.entityKind; - if (readerKind == EntityKind_t::BUILD_IN_READER_WITH_KEY || - readerKind == EntityKind_t::BUILD_IN_READER_WITHOUT_KEY) { - return; // No need to announce builtin endpoints - } + EntityKind_t readerKind = + reader.m_attributes.endpointGuid.entityId.entityKind; + if (readerKind == EntityKind_t::BUILD_IN_READER_WITH_KEY || + readerKind == EntityKind_t::BUILD_IN_READER_WITHOUT_KEY) { + return; // No need to announce builtin endpoints + } - Lock lock{m_mutex}; - ucdrBuffer microbuffer; - ucdr_init_buffer(µbuffer, m_buffer, - sizeof(m_buffer) / sizeof(m_buffer[0])); - const uint16_t zero_options = 0; + Lock lock{m_mutex}; + ucdrBuffer microbuffer; + ucdr_init_buffer(µbuffer, m_buffer, + sizeof(m_buffer) / sizeof(m_buffer[0])); + const uint16_t zero_options = 0; - ucdr_serialize_array_uint8_t(µbuffer, - rtps::SMElement::SCHEME_PL_CDR_LE.data(), - rtps::SMElement::SCHEME_PL_CDR_LE.size()); - ucdr_serialize_uint16_t(µbuffer, zero_options); - reader.m_attributes.serializeIntoUcdrBuffer(microbuffer); - m_endpoints.sedpSubWriter->newChange(ChangeKind_t::ALIVE, m_buffer, - ucdr_buffer_length(µbuffer)); + ucdr_serialize_array_uint8_t(µbuffer, + rtps::SMElement::SCHEME_PL_CDR_LE.data(), + rtps::SMElement::SCHEME_PL_CDR_LE.size()); + ucdr_serialize_uint16_t(µbuffer, zero_options); + reader.m_attributes.serializeIntoUcdrBuffer(microbuffer); + m_endpoints.sedpSubWriter->newChange(ChangeKind_t::ALIVE, m_buffer, + ucdr_buffer_length(µbuffer)); #if SEDP_VERBOSE - SEDP_LOG("Added new change to sedpSubWriter.\n"); + SEDP_LOG("Added new change to sedpSubWriter.\n"); #endif -} +} \ No newline at end of file diff --git a/src/discovery/TopicData.cpp b/src/discovery/TopicData.cpp index 9caf2b00..b47d31a4 100644 --- a/src/discovery/TopicData.cpp +++ b/src/discovery/TopicData.cpp @@ -86,15 +86,13 @@ bool TopicData::readFromUcdrBuffer(ucdrBuffer &buffer) { case ParameterId::PID_OWNERSHIP: uint32_t ownershipKindLength; ucdr_deserialize_uint32_t(&buffer, &ownershipKindLength); - if(ownershipKindLength == 1) { - ucdr_deserialize_uint8_t(&buffer, reinterpret_cast(&ownership_Kind)); - } +// ucdr_deserialize_uint8_t(&buffer, reinterpret_cast(&ownership_Kind)); + break; case ParameterId::PID_OWNERSHIP_STRENGTH: uint32_t ownershipStrenghtLength; ucdr_deserialize_uint32_t(&buffer, &ownershipStrenghtLength); - if(ownershipStrenghtLength == 4) - ucdr_deserialize_uint32_t(&buffer, &ownership_strenght); + //ucdr_deserialize_uint32_t(&buffer, &ownership_strenght); break; default: buffer.iterator += length; @@ -192,10 +190,7 @@ bool TopicData::serializeIntoUcdrBuffer(ucdrBuffer &buffer) const { ucdr_serialize_uint16_t(&buffer, ParameterId::PID_DURABILITY); ucdr_serialize_uint16_t(&buffer, sizeof(DurabilityKind_t)); ucdr_serialize_uint32_t(&buffer, static_cast(durabilityKind)); - - ucdr_serialize_uint16_t(&buffer, ParameterId::PID_SENTINEL); - ucdr_serialize_uint16_t(&buffer, 0); - //Ownership +/* ucdr_serialize_uint16_t(&buffer, ParameterId::PID_OWNERSHIP); ucdr_serialize_uint16_t(&buffer, sizeof(OwnershipKind_t)); ucdr_serialize_uint8_t(&buffer, static_cast(ownership_Kind)); @@ -205,6 +200,10 @@ bool TopicData::serializeIntoUcdrBuffer(ucdrBuffer &buffer) const { ucdr_serialize_uint16_t(&buffer, sizeof(OwnershipStrength)); ucdr_serialize_uint32_t(&buffer, ownership_strenght); } + */ + //End of QoS + ucdr_serialize_uint16_t(&buffer, ParameterId::PID_SENTINEL); + ucdr_serialize_uint16_t(&buffer, 0); return true; } diff --git a/src/entities/Domain.cpp b/src/entities/Domain.cpp index 646ca2cf..3dbc99a9 100644 --- a/src/entities/Domain.cpp +++ b/src/entities/Domain.cpp @@ -336,7 +336,7 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, attributes.unicastLocator = getUserUnicastLocator(part.m_participantId); attributes.durabilityKind = DurabilityKind_t::TRANSIENT_LOCAL; - attributes.ownership_Kind = OwnershipKind_t::SHARED; +// attributes.ownership_Kind = OwnershipKind_t::SHARED; DOMAIN_LOG("Creating writer[%s, %s]\n", topicName, typeName); if (reliable) { @@ -360,52 +360,6 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, } } -rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, - const char *typeName, OwnershipKind_t ownership_kind, OwnershipStrength ownership_strenght, - bool enforceUnicast){ - // Check if there is enough capacity for more writers Ownership currently just support reliable writer so its compatible with fastDDS (dont really get why thats a problem) - if (m_statefulWriters.size() <= m_numStatefulWriters && (ownership_kind == OwnershipKind_t::EXCLUSIVE)) { - DOMAIN_LOG("No Writer created. Max Number of Writers reached.\n"); - return nullptr; - } - if(OwnershipKind_t::SHARED == ownership_kind) { - return createWriter(part, topicName, typeName, false, enforceUnicast); - } - // TODO Distinguish WithKey and NoKey (Also changes EntityKind) - TopicData attributes; - - if (strlen(topicName) > Config::MAX_TOPICNAME_LENGTH || - strlen(typeName) > Config::MAX_TYPENAME_LENGTH) { - return nullptr; - } - strcpy(attributes.topicName, topicName); - strcpy(attributes.typeName, typeName); - attributes.endpointGuid.prefix = part.m_guidPrefix; - attributes.endpointGuid.entityId = { - part.getNextUserEntityKey(), - EntityKind_t::USER_DEFINED_WRITER_WITHOUT_KEY}; - attributes.unicastLocator = getUserUnicastLocator(part.m_participantId); - attributes.durabilityKind = DurabilityKind_t::TRANSIENT_LOCAL; - - DOMAIN_LOG("Creating writer[%s, %s]\n", topicName, typeName); - - attributes.reliabilityKind = ReliabilityKind_t::RELIABLE; - - attributes.ownership_Kind = ownership_kind; - attributes.ownership_strenght = ownership_strenght; - attributes.topicKind = TopicKind_t::WITH_KEY; - if(attributes.topicKind == TopicKind_t::WITH_KEY){ - attributes.endpointGuid.entityId.entityKind = EntityKind_t::USER_DEFINED_WRITER_WITH_KEY; - } - StatefulWriter &writer = m_statefulWriters[m_numStatefulWriters++]; - writer.init(attributes, attributes.topicKind, &m_threadPool, m_transport, - enforceUnicast); - - part.addWriter(&writer); - return &writer; - -} - rtps::Reader *Domain::createReader(Participant &part, const char *topicName, const char *typeName, bool reliable, ip4_addr_t mcastaddress) { From 65c728ae54cf4f730e8c68a1d8f22b397a4d13fb Mon Sep 17 00:00:00 2001 From: sven Date: Tue, 5 Oct 2021 15:47:55 +0200 Subject: [PATCH 13/25] discovery writer --- include/rtps/common/types.h | 2 +- include/rtps/discovery/TopicData.h | 22 ++------- include/rtps/entities/Domain.h | 5 +- include/rtps/entities/StatefulWriter.tpp | 9 +++- include/rtps/messages/MessageFactory.h | 4 +- include/rtps/messages/MessageTypes.h | 10 ++-- src/discovery/TopicData.cpp | 14 +++--- src/entities/Domain.cpp | 62 +++++++++++++++--------- src/entities/Participant.cpp | 4 +- 9 files changed, 73 insertions(+), 59 deletions(-) diff --git a/include/rtps/common/types.h b/include/rtps/common/types.h index f1da4780..ca721d84 100644 --- a/include/rtps/common/types.h +++ b/include/rtps/common/types.h @@ -89,7 +89,7 @@ enum class OwnershipKind_t : uint8_t{ EXCLUSIVE = 1 }; -typedef uint32_t OwnershipStrength; +typedef uint32_t OwnershipStrength_t; struct GuidPrefix_t { std::array id; diff --git a/include/rtps/discovery/TopicData.h b/include/rtps/discovery/TopicData.h index 13502b4d..650e8546 100644 --- a/include/rtps/discovery/TopicData.h +++ b/include/rtps/discovery/TopicData.h @@ -46,9 +46,8 @@ struct TopicData { DurabilityKind_t durabilityKind; Locator unicastLocator; Locator multicastLocator; - //OwnershipKind_t ownership_Kind; - //OwnershipStrength ownership_strenght; - //TopicKind_t topicKind; + OwnershipKind_t ownership_Kind; + OwnershipStrength_t ownership_strenght; TopicData() : endpointGuid(GUID_UNKNOWN), typeName{'\0'}, topicName{'\0'}, reliabilityKind(ReliabilityKind_t::BEST_EFFORT), @@ -57,9 +56,8 @@ struct TopicData { 192, 168, 0, 42, rtps::getUserUnicastPort(0)); unicastLocator = someLocator; multicastLocator = Locator(); - //ownership_Kind = OwnershipKind_t::SHARED; - // ownership_strenght = 0; - // topicKind = TopicKind_t::NO_KEY; + ownership_Kind = OwnershipKind_t::SHARED; + ownership_strenght = 0; }; TopicData(Guid_t guid, ReliabilityKind_t reliability, Locator loc) @@ -70,18 +68,6 @@ struct TopicData { //topicKind = TopicKind_t::NO_KEY; }; - TopicData(Guid_t guid, OwnershipKind_t kind, OwnershipStrength strength, Locator loc) - : endpointGuid(guid), typeName{'\0'}, topicName{'\0'}, - // ownership_Kind(kind), ownership_strenght(strength), - durabilityKind(DurabilityKind_t::TRANSIENT_LOCAL), unicastLocator(loc) { - - //if(ownership_Kind == OwnershipKind_t::EXCLUSIVE ) { - // reliabilityKind = (ReliabilityKind_t::RELIABLE); - // topicKind = TopicKind_t::WITH_KEY; - // endpointGuid.entityId.entityKind = EntityKind_t::USER_DEFINED_WRITER_WITH_KEY; - //} - }; - bool matchesTopicOf(const TopicData &other); bool readFromUcdrBuffer(ucdrBuffer &buffer); diff --git a/include/rtps/entities/Domain.h b/include/rtps/entities/Domain.h index 1af7607b..aa0f8118 100644 --- a/include/rtps/entities/Domain.h +++ b/include/rtps/entities/Domain.h @@ -51,14 +51,17 @@ class Domain { bool enforceUnicast = false); Writer *createWriter(Participant &part, const char *topicName, - const char *typeName, OwnershipKind_t ownership_kind, OwnershipStrength ownership_strenght, + const char *typeName, OwnershipStrength_t ownership_strenght, bool enforceUnicast = false); + Writer *createWriter(Participant &part, const char *topicName, const char *typeName, OwnershipKind_t ownership_kind, OwnershipStrength_t ownershipStrength, bool reliable , bool topichasKey , bool enforceUnicast); + Reader *createReader(Participant &part, const char *topicName, const char *typeName, bool reliable, ip4_addr_t mcastaddress = {0}); + Reader *createReader(Participant &part, const char *topicName, const char *typeName, OwnershipKind_t ownership_Kind, ip4_addr_t mcastaddress = {0}); diff --git a/include/rtps/entities/StatefulWriter.tpp b/include/rtps/entities/StatefulWriter.tpp index 19e3d2db..da333239 100644 --- a/include/rtps/entities/StatefulWriter.tpp +++ b/include/rtps/entities/StatefulWriter.tpp @@ -357,9 +357,14 @@ bool StatefulWriterT::sendData( return false; } + bool inlineQoS = false; + if(m_attributes.ownership_Kind == OwnershipKind_t::EXCLUSIVE){ + inlineQoS = true; + } MessageFactory::addSubMessageData( - info.buffer, next->data, false, next->sequenceNumber, - m_attributes.endpointGuid.entityId, reader.remoteReaderGuid.entityId); + info.buffer, next->data, inlineQoS, next->sequenceNumber, + m_attributes.endpointGuid.entityId, reader.remoteReaderGuid.entityId, m_attributes.ownership_Kind, m_attributes.ownership_strenght + ); } m_transport->sendPacket(info); diff --git a/include/rtps/messages/MessageFactory.h b/include/rtps/messages/MessageFactory.h index 87275e39..f253d350 100644 --- a/include/rtps/messages/MessageFactory.h +++ b/include/rtps/messages/MessageFactory.h @@ -82,7 +82,7 @@ namespace rtps { template void addSubMessageData(Buffer &buffer, const Buffer &filledPayload, bool containsInlineQos, const SequenceNumber_t &SN, - const EntityId_t &writerID, const EntityId_t &readerID) { + const EntityId_t &writerID, const EntityId_t &readerID, const OwnershipKind_t ownershipKind = OwnershipKind_t::SHARED, const OwnershipStrength_t ownershipStrength = 0) { SubmessageData msg; msg.header.submessageId = SubmessageKind::DATA; #if IS_LITTLE_ENDIAN @@ -107,6 +107,8 @@ namespace rtps { msg.readerId = readerID; msg.writerId = writerID; + msg.ownershipKind = ownershipKind; + msg.ownershipStrength = ownershipStrength; constexpr uint16_t octetsToInlineQoS = 4 + 4 + 8; // EntityIds + SequenceNumber msg.octetsToInlineQos = octetsToInlineQoS; diff --git a/include/rtps/messages/MessageTypes.h b/include/rtps/messages/MessageTypes.h index 2bfe01f0..ada9bd69 100644 --- a/include/rtps/messages/MessageTypes.h +++ b/include/rtps/messages/MessageTypes.h @@ -177,7 +177,7 @@ struct SubmessageData { SequenceNumber_t writerSN; OwnershipKind_t ownershipKind; - OwnershipStrength ownershipStrength; + OwnershipStrength_t ownershipStrength; static constexpr uint16_t getRawSize() { return SubmessageHeader::getRawSize() + sizeof(uint16_t) + sizeof(uint16_t) + (2 * 3 + 2 * 1) // EntityID @@ -268,14 +268,14 @@ bool serializeMessage(Buffer &buffer, SubmessageData &msg) { sizeof(msg.writerSN.high)); buffer.append(reinterpret_cast(&msg.writerSN.low), sizeof(msg.writerSN.low)); - /*if(msg.ownershipKind == OwnershipKind_t::EXCLUSIVE){ + if(msg.ownershipKind == OwnershipKind_t::EXCLUSIVE){ buffer.append(reinterpret_cast(SMElement::ParameterId::PID_OWNERSHIP_STRENGTH), sizeof(uint16_t)); - buffer.append(reinterpret_cast(sizeof(OwnershipStrength)), sizeof(OwnershipStrength)); - buffer.append(reinterpret_cast(msg.ownershipStrength), sizeof(OwnershipStrength)); + buffer.append(reinterpret_cast(sizeof(OwnershipStrength_t)), sizeof(uint16_t)); + buffer.append(reinterpret_cast(msg.ownershipStrength), sizeof(OwnershipStrength_t)); buffer.append(reinterpret_cast(SMElement::ParameterId::PID_KEY_HASH), sizeof(uint16_t)); uint16_t key = 1; buffer.append(reinterpret_cast(key), sizeof(uint16_t)); - }*/ + } return true; } diff --git a/src/discovery/TopicData.cpp b/src/discovery/TopicData.cpp index b47d31a4..4148c593 100644 --- a/src/discovery/TopicData.cpp +++ b/src/discovery/TopicData.cpp @@ -29,8 +29,10 @@ using rtps::TopicData; using rtps::SMElement::ParameterId; bool TopicData::matchesTopicOf(const TopicData &other) { - return strcmp(this->topicName, other.topicName) == 0 && + bool sameTopicAndType = strcmp(this->topicName, other.topicName) == 0 && strcmp(this->typeName, other.typeName) == 0; + + return sameTopicAndType; } bool TopicData::readFromUcdrBuffer(ucdrBuffer &buffer) { @@ -62,8 +64,6 @@ bool TopicData::readFromUcdrBuffer(ucdrBuffer &buffer) { buffer.iterator += 8; // TODO Skip 8 bytes. don't know what they are yet break; - case ParameterId::PID_SENTINEL: - return true; case ParameterId::PID_TOPIC_NAME: uint32_t topicNameLength; ucdr_deserialize_uint32_t(&buffer, &topicNameLength); @@ -94,6 +94,8 @@ bool TopicData::readFromUcdrBuffer(ucdrBuffer &buffer) { ucdr_deserialize_uint32_t(&buffer, &ownershipStrenghtLength); //ucdr_deserialize_uint32_t(&buffer, &ownership_strenght); break; + case ParameterId::PID_SENTINEL: + return true;//End of information default: buffer.iterator += length; buffer.last_data_size = 1; @@ -190,17 +192,17 @@ bool TopicData::serializeIntoUcdrBuffer(ucdrBuffer &buffer) const { ucdr_serialize_uint16_t(&buffer, ParameterId::PID_DURABILITY); ucdr_serialize_uint16_t(&buffer, sizeof(DurabilityKind_t)); ucdr_serialize_uint32_t(&buffer, static_cast(durabilityKind)); -/* + ucdr_serialize_uint16_t(&buffer, ParameterId::PID_OWNERSHIP); ucdr_serialize_uint16_t(&buffer, sizeof(OwnershipKind_t)); ucdr_serialize_uint8_t(&buffer, static_cast(ownership_Kind)); if(ownership_Kind == OwnershipKind_t::EXCLUSIVE) { ucdr_serialize_uint16_t(&buffer, ParameterId::PID_OWNERSHIP_STRENGTH); - ucdr_serialize_uint16_t(&buffer, sizeof(OwnershipStrength)); + ucdr_serialize_uint16_t(&buffer, sizeof(ownership_strenght)); ucdr_serialize_uint32_t(&buffer, ownership_strenght); } - */ + //End of QoS ucdr_serialize_uint16_t(&buffer, ParameterId::PID_SENTINEL); ucdr_serialize_uint16_t(&buffer, 0); diff --git a/src/entities/Domain.cpp b/src/entities/Domain.cpp index 3dbc99a9..50fd92e1 100644 --- a/src/entities/Domain.cpp +++ b/src/entities/Domain.cpp @@ -306,21 +306,19 @@ rtps::Writer *Domain::writerExists(Participant &part, const char *topicName, return nullptr; } -rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, - const char *typeName, bool reliable, - bool enforceUnicast) { - - // Check if there is enough capacity for more writers - if ((reliable && m_statefulWriters.size() <= m_numStatefulWriters) || - (!reliable && m_statelessWriters.size() <= m_numStatelessWriters) || - part.isWritersFull()) { - - DOMAIN_LOG("No Writer created. Max Number of Writers reached.\n"); - - return nullptr; - } +rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, const char *typeName, + OwnershipKind_t ownership_kind, OwnershipStrength_t ownershipStrength, + bool reliable , + bool topichasKey, + bool enforceUnicast ){ + // Check if there is enough capacity for more writers + if (((reliable || ownership_kind == OwnershipKind_t::EXCLUSIVE) && m_statefulWriters.size() <= m_numStatefulWriters) || //Ownership needs reliable writer + ((!reliable) && m_statelessWriters.size() <= m_numStatelessWriters) || + part.isWritersFull()) { + DOMAIN_LOG("No Writer created. Max Number of Writers reached.\n"); + return nullptr; + } - // TODO Distinguish WithKey and NoKey (Also changes EntityKind) TopicData attributes; if (strlen(topicName) > Config::MAX_TOPICNAME_LENGTH || @@ -331,19 +329,25 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, strcpy(attributes.typeName, typeName); attributes.endpointGuid.prefix = part.m_guidPrefix; attributes.endpointGuid.entityId = { - part.getNextUserEntityKey(), - EntityKind_t::USER_DEFINED_WRITER_WITHOUT_KEY}; + part.getNextUserEntityKey(), + EntityKind_t::USER_DEFINED_WRITER_WITHOUT_KEY}; + + TopicKind_t kind = TopicKind_t::NO_KEY; + if(topichasKey){ + kind = TopicKind_t::WITH_KEY; + attributes.endpointGuid.entityId.entityKind = EntityKind_t::USER_DEFINED_WRITER_WITH_KEY; + } attributes.unicastLocator = getUserUnicastLocator(part.m_participantId); attributes.durabilityKind = DurabilityKind_t::TRANSIENT_LOCAL; -// attributes.ownership_Kind = OwnershipKind_t::SHARED; - DOMAIN_LOG("Creating writer[%s, %s]\n", topicName, typeName); - - if (reliable) { + if (reliable || ownership_kind == OwnershipKind_t::EXCLUSIVE) { attributes.reliabilityKind = ReliabilityKind_t::RELIABLE; - + if(ownership_kind == OwnershipKind_t::EXCLUSIVE){ + attributes.ownership_Kind = OwnershipKind_t::EXCLUSIVE; + attributes.ownership_strenght = ownershipStrength; + } StatefulWriter &writer = m_statefulWriters[m_numStatefulWriters++]; - writer.init(attributes, TopicKind_t::NO_KEY, &m_threadPool, m_transport, + writer.init(attributes, kind, &m_threadPool, m_transport, enforceUnicast); part.addWriter(&writer); @@ -352,7 +356,7 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, attributes.reliabilityKind = ReliabilityKind_t::BEST_EFFORT; StatelessWriter &writer = m_statelessWriters[m_numStatelessWriters++]; - writer.init(attributes, TopicKind_t::NO_KEY, &m_threadPool, m_transport, + writer.init(attributes, kind, &m_threadPool, m_transport, enforceUnicast); part.addWriter(&writer); @@ -360,6 +364,18 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, } } +rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, + const char *typeName, OwnershipStrength_t ownership_strenght, + bool enforceUnicast ){ + return createWriter(part, topicName,typeName, OwnershipKind_t::EXCLUSIVE, ownership_strenght, true, true, enforceUnicast); +} + +rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, + const char *typeName, bool reliable, + bool enforceUnicast) { + return createWriter(part,topicName,typeName,OwnershipKind_t::SHARED, 0, reliable, false, enforceUnicast); +} + rtps::Reader *Domain::createReader(Participant &part, const char *topicName, const char *typeName, bool reliable, ip4_addr_t mcastaddress) { diff --git a/src/entities/Participant.cpp b/src/entities/Participant.cpp index c821f0c5..d9ed43c3 100644 --- a/src/entities/Participant.cpp +++ b/src/entities/Participant.cpp @@ -166,8 +166,8 @@ Participant::getMatchingWriter(const TopicData &readerTopicData) const { for (uint8_t i = 0; i < m_numWriters; ++i) { if (m_writers[i]->m_attributes.matchesTopicOf(readerTopicData) && (readerTopicData.reliabilityKind == ReliabilityKind_t::BEST_EFFORT || - m_writers[i]->m_attributes.reliabilityKind == - ReliabilityKind_t::RELIABLE)) { + m_writers[i]->m_attributes.reliabilityKind == ReliabilityKind_t::RELIABLE || + m_writers[i]->m_attributes.ownership_Kind == readerTopicData.ownership_Kind)) { return m_writers[i]; } } From 5e5af1eb7cafbc1234b648a91b78fef116e0c6f8 Mon Sep 17 00:00:00 2001 From: sven Date: Tue, 5 Oct 2021 19:19:59 +0200 Subject: [PATCH 14/25] discovery writer fix for fast dds --- include/rtps/common/types.h | 2 +- include/rtps/messages/MessageTypes.h | 2 ++ src/discovery/TopicData.cpp | 2 +- 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/include/rtps/common/types.h b/include/rtps/common/types.h index ca721d84..09268f8a 100644 --- a/include/rtps/common/types.h +++ b/include/rtps/common/types.h @@ -84,7 +84,7 @@ enum class DurabilityKind_t : uint32_t { PERSISTENT = 3 }; -enum class OwnershipKind_t : uint8_t{ +enum class OwnershipKind_t : uint32_t{ SHARED = 0, EXCLUSIVE = 1 }; diff --git a/include/rtps/messages/MessageTypes.h b/include/rtps/messages/MessageTypes.h index ada9bd69..45ad0d2d 100644 --- a/include/rtps/messages/MessageTypes.h +++ b/include/rtps/messages/MessageTypes.h @@ -275,6 +275,8 @@ bool serializeMessage(Buffer &buffer, SubmessageData &msg) { buffer.append(reinterpret_cast(SMElement::ParameterId::PID_KEY_HASH), sizeof(uint16_t)); uint16_t key = 1; buffer.append(reinterpret_cast(key), sizeof(uint16_t)); + buffer.append(reinterpret_cast(SMElement::ParameterId::PID_SENTINEL), sizeof(uint16_t)); + buffer.append(reinterpret_cast((uint16_t) 0), sizeof(uint16_t)); } return true; } diff --git a/src/discovery/TopicData.cpp b/src/discovery/TopicData.cpp index 4148c593..7cca1b3e 100644 --- a/src/discovery/TopicData.cpp +++ b/src/discovery/TopicData.cpp @@ -195,7 +195,7 @@ bool TopicData::serializeIntoUcdrBuffer(ucdrBuffer &buffer) const { ucdr_serialize_uint16_t(&buffer, ParameterId::PID_OWNERSHIP); ucdr_serialize_uint16_t(&buffer, sizeof(OwnershipKind_t)); - ucdr_serialize_uint8_t(&buffer, static_cast(ownership_Kind)); + ucdr_serialize_uint32_t(&buffer, static_cast(ownership_Kind)); if(ownership_Kind == OwnershipKind_t::EXCLUSIVE) { ucdr_serialize_uint16_t(&buffer, ParameterId::PID_OWNERSHIP_STRENGTH); From 3ad8b3e665d0a5097cf413cfcc91349d7298f334 Mon Sep 17 00:00:00 2001 From: sven Date: Wed, 6 Oct 2021 13:35:09 +0200 Subject: [PATCH 15/25] Reader discovery works --- include/rtps/common/types.h | 4 +++ include/rtps/discovery/TopicData.h | 2 -- include/rtps/entities/Domain.h | 8 ++--- include/rtps/entities/StatefulReader.h | 2 +- include/rtps/entities/StatefulReader.tpp | 5 +++ include/rtps/entities/Writer.h | 1 + include/rtps/messages/MessageTypes.h | 9 +++-- src/discovery/TopicData.cpp | 10 ++---- src/entities/Domain.cpp | 42 +++++++++++++++--------- 9 files changed, 51 insertions(+), 32 deletions(-) diff --git a/include/rtps/common/types.h b/include/rtps/common/types.h index 09268f8a..2e7f368e 100644 --- a/include/rtps/common/types.h +++ b/include/rtps/common/types.h @@ -40,6 +40,10 @@ typedef uint16_t Ip4Port_t; typedef uint16_t DataSize_t; typedef int8_t ParticipantId_t; // With UDP only 120 possible +class DynamicDataType{ + virtual void getKeyAttributes(uint8_t *buff, uint32_t &len) = 0;//Put all key attributes into an array +}; + enum class EntityKind_t : uint8_t { USER_DEFINED_UNKNOWN = 0x00, // No user define participant diff --git a/include/rtps/discovery/TopicData.h b/include/rtps/discovery/TopicData.h index 650e8546..ae99c779 100644 --- a/include/rtps/discovery/TopicData.h +++ b/include/rtps/discovery/TopicData.h @@ -64,8 +64,6 @@ struct TopicData { : endpointGuid(guid), typeName{'\0'}, topicName{'\0'}, reliabilityKind(reliability), durabilityKind(DurabilityKind_t::TRANSIENT_LOCAL), unicastLocator(loc) { - // ownership_Kind(OwnershipKind_t::SHARED), ownership_strenght(0){ - //topicKind = TopicKind_t::NO_KEY; }; bool matchesTopicOf(const TopicData &other); diff --git a/include/rtps/entities/Domain.h b/include/rtps/entities/Domain.h index aa0f8118..18f95a0b 100644 --- a/include/rtps/entities/Domain.h +++ b/include/rtps/entities/Domain.h @@ -57,15 +57,15 @@ class Domain { Writer *createWriter(Participant &part, const char *topicName, const char *typeName, OwnershipKind_t ownership_kind, OwnershipStrength_t ownershipStrength, bool reliable , bool topichasKey , bool enforceUnicast); + Reader *createReader(Participant &part, const char *topicName, bool topichasKey, + const char *typeName, bool reliable, OwnershipKind_t ownershipKind, + ip4_addr_t mcastaddress = {0}); + Reader *createReader(Participant &part, const char *topicName, const char *typeName, bool reliable, ip4_addr_t mcastaddress = {0}); - Reader *createReader(Participant &part, const char *topicName, - const char *typeName, OwnershipKind_t ownership_Kind, - ip4_addr_t mcastaddress = {0}); - Writer *writerExists(Participant &part, const char *topicName, const char *typeName, bool reliable); Reader *readerExists(Participant &part, const char *topicName, diff --git a/include/rtps/entities/StatefulReader.h b/include/rtps/entities/StatefulReader.h index fc73892e..fe08eb15 100644 --- a/include/rtps/entities/StatefulReader.h +++ b/include/rtps/entities/StatefulReader.h @@ -51,7 +51,7 @@ template class StatefulReaderT final : public Reader { PacketInfo m_packetInfo; // TODO intended for reuse but buffer not used as such NetworkDriver *m_transport; - + TopicKind_t m_kind = TopicKind_t::NO_KEY; ddsReaderCallback_fp m_callback = nullptr; void *m_callee = nullptr; sys_mutex_t m_mutex; diff --git a/include/rtps/entities/StatefulReader.tpp b/include/rtps/entities/StatefulReader.tpp index a8037f71..a1c36d0d 100644 --- a/include/rtps/entities/StatefulReader.tpp +++ b/include/rtps/entities/StatefulReader.tpp @@ -59,6 +59,11 @@ void StatefulReaderT::init(const TopicData &attributes, return; } + + if(attributes.endpointGuid.entityId.entityKind == EntityKind_t::USER_DEFINED_READER_WITH_KEY){ + m_kind = TopicKind_t::WITH_KEY; + } + m_attributes = attributes; m_transport = &driver; m_packetInfo.srcPort = attributes.unicastLocator.port; diff --git a/include/rtps/entities/Writer.h b/include/rtps/entities/Writer.h index c300bcae..2a0552af 100644 --- a/include/rtps/entities/Writer.h +++ b/include/rtps/entities/Writer.h @@ -46,6 +46,7 @@ class Writer { virtual void progress() = 0; virtual const CacheChange *newChange(ChangeKind_t kind, const uint8_t *data, DataSize_t size) = 0; + // virtual const CacheChange *newChange(ChangeKind_t kind, DynamicDataType *data) = 0; virtual void setAllChangesToUnsent() = 0; virtual void onNewAckNack(const SubmessageAckNack &msg, const GuidPrefix_t &sourceGuidPrefix) = 0; diff --git a/include/rtps/messages/MessageTypes.h b/include/rtps/messages/MessageTypes.h index 45ad0d2d..a465e17e 100644 --- a/include/rtps/messages/MessageTypes.h +++ b/include/rtps/messages/MessageTypes.h @@ -273,10 +273,13 @@ bool serializeMessage(Buffer &buffer, SubmessageData &msg) { buffer.append(reinterpret_cast(sizeof(OwnershipStrength_t)), sizeof(uint16_t)); buffer.append(reinterpret_cast(msg.ownershipStrength), sizeof(OwnershipStrength_t)); buffer.append(reinterpret_cast(SMElement::ParameterId::PID_KEY_HASH), sizeof(uint16_t)); - uint16_t key = 1; - buffer.append(reinterpret_cast(key), sizeof(uint16_t)); + uint8_t key[16]; + buffer.append(reinterpret_cast(16),sizeof(uint16_t)); + buffer.append(reinterpret_cast(key), 16); + buffer.append(reinterpret_cast(SMElement::ParameterId::PID_SENTINEL), sizeof(uint16_t)); - buffer.append(reinterpret_cast((uint16_t) 0), sizeof(uint16_t)); + buffer.append(reinterpret_cast(0), sizeof(uint16_t)); + } return true; } diff --git a/src/discovery/TopicData.cpp b/src/discovery/TopicData.cpp index 7cca1b3e..0068138b 100644 --- a/src/discovery/TopicData.cpp +++ b/src/discovery/TopicData.cpp @@ -84,15 +84,11 @@ bool TopicData::readFromUcdrBuffer(ucdrBuffer &buffer) { multicastLocator.readFromUcdrBuffer(buffer); break; case ParameterId::PID_OWNERSHIP: - uint32_t ownershipKindLength; - ucdr_deserialize_uint32_t(&buffer, &ownershipKindLength); -// ucdr_deserialize_uint8_t(&buffer, reinterpret_cast(&ownership_Kind)); + ucdr_deserialize_uint32_t(&buffer,reinterpret_cast(&ownership_Kind)); break; case ParameterId::PID_OWNERSHIP_STRENGTH: - uint32_t ownershipStrenghtLength; - ucdr_deserialize_uint32_t(&buffer, &ownershipStrenghtLength); - //ucdr_deserialize_uint32_t(&buffer, &ownership_strenght); + ucdr_deserialize_uint32_t(&buffer,reinterpret_cast(&ownership_strenght)); break; case ParameterId::PID_SENTINEL: return true;//End of information @@ -197,7 +193,7 @@ bool TopicData::serializeIntoUcdrBuffer(ucdrBuffer &buffer) const { ucdr_serialize_uint16_t(&buffer, sizeof(OwnershipKind_t)); ucdr_serialize_uint32_t(&buffer, static_cast(ownership_Kind)); - if(ownership_Kind == OwnershipKind_t::EXCLUSIVE) { + if(ownership_Kind == OwnershipKind_t::EXCLUSIVE && endpointGuid.entityId.entityKind == EntityKind_t::USER_DEFINED_WRITER_WITH_KEY) {//Only writers have strength ucdr_serialize_uint16_t(&buffer, ParameterId::PID_OWNERSHIP_STRENGTH); ucdr_serialize_uint16_t(&buffer, sizeof(ownership_strenght)); ucdr_serialize_uint32_t(&buffer, ownership_strenght); diff --git a/src/entities/Domain.cpp b/src/entities/Domain.cpp index 50fd92e1..d7ec5cd0 100644 --- a/src/entities/Domain.cpp +++ b/src/entities/Domain.cpp @@ -306,6 +306,7 @@ rtps::Writer *Domain::writerExists(Participant &part, const char *topicName, return nullptr; } + rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, const char *typeName, OwnershipKind_t ownership_kind, OwnershipStrength_t ownershipStrength, bool reliable , @@ -376,10 +377,10 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, return createWriter(part,topicName,typeName,OwnershipKind_t::SHARED, 0, reliable, false, enforceUnicast); } -rtps::Reader *Domain::createReader(Participant &part, const char *topicName, - const char *typeName, bool reliable, - ip4_addr_t mcastaddress) { - if ((reliable && m_statefulReaders.size() <= m_numStatefulReaders) || +rtps::Reader *Domain::createReader(Participant &part, const char *topicName, bool topichasKey, + const char *typeName, bool reliable, OwnershipKind_t ownershipKind, + ip4_addr_t mcastaddress){ + if (((reliable || (ownershipKind == OwnershipKind_t::EXCLUSIVE) )&& m_statefulReaders.size() <= m_numStatefulReaders) || (!reliable && m_statelessReaders.size() <= m_numStatelessReaders) || part.isReadersFull()) { @@ -387,8 +388,6 @@ rtps::Reader *Domain::createReader(Participant &part, const char *topicName, return nullptr; } - - // TODO Distinguish WithKey and NoKey (Also changes EntityKind) TopicData attributes; if (strlen(topicName) > Config::MAX_TOPICNAME_LENGTH || @@ -399,17 +398,17 @@ rtps::Reader *Domain::createReader(Participant &part, const char *topicName, strcpy(attributes.typeName, typeName); attributes.endpointGuid.prefix = part.m_guidPrefix; attributes.endpointGuid.entityId = { - part.getNextUserEntityKey(), - EntityKind_t::USER_DEFINED_READER_WITHOUT_KEY}; + part.getNextUserEntityKey(), + EntityKind_t::USER_DEFINED_READER_WITHOUT_KEY}; attributes.unicastLocator = getUserUnicastLocator(part.m_participantId); if (!isZeroAddress(mcastaddress)) { if (ip4_addr_ismulticast(&mcastaddress)) { attributes.multicastLocator = rtps::Locator::createUDPv4Locator( - ip4_addr1(&mcastaddress), ip4_addr2(&mcastaddress), - ip4_addr3(&mcastaddress), ip4_addr4(&mcastaddress), - getUserMulticastPort()); + ip4_addr1(&mcastaddress), ip4_addr2(&mcastaddress), + ip4_addr3(&mcastaddress), ip4_addr4(&mcastaddress), + getUserMulticastPort()); m_transport.joinMultiCastGroup( - attributes.multicastLocator.getIp4Address()); + attributes.multicastLocator.getIp4Address()); registerMulticastPort(attributes.multicastLocator); DOMAIN_LOG("Multicast enabled!\n"); @@ -420,16 +419,20 @@ rtps::Reader *Domain::createReader(Participant &part, const char *topicName, } } attributes.durabilityKind = DurabilityKind_t::VOLATILE; - + TopicKind_t kind; + if(topichasKey){ + kind = TopicKind_t::WITH_KEY; + attributes.endpointGuid.entityId.entityKind = EntityKind_t::USER_DEFINED_READER_WITH_KEY; + } DOMAIN_LOG("Creating reader[%s, %s]\n", topicName, typeName); - if (reliable) { + if (reliable || (ownershipKind == OwnershipKind_t::EXCLUSIVE)) { if (m_numStatefulReaders == m_statefulReaders.size()) { return nullptr; } attributes.reliabilityKind = ReliabilityKind_t::RELIABLE; - + attributes.ownership_Kind = ownershipKind; StatefulReader &reader = m_statefulReaders[m_numStatefulReaders++]; reader.init(attributes, m_transport); @@ -452,8 +455,17 @@ rtps::Reader *Domain::createReader(Participant &part, const char *topicName, } return &reader; } + +} + +rtps::Reader *Domain::createReader(Participant &part, const char *topicName, + const char *typeName, bool reliable, + ip4_addr_t mcastaddress) { + return createReader(part,topicName, false,typeName, reliable, OwnershipKind_t::SHARED, mcastaddress); } + + rtps::GuidPrefix_t Domain::generateGuidPrefix(ParticipantId_t id) const { GuidPrefix_t prefix = Config::BASE_GUID_PREFIX; prefix.id[prefix.id.size() - 1] = *reinterpret_cast(&id); From 9e09d0dd3a797823341e313a4a3c3aa4396b6e85 Mon Sep 17 00:00:00 2001 From: sven Date: Wed, 6 Oct 2021 17:50:32 +0200 Subject: [PATCH 16/25] messages at fast dds dont throw exception anymore --- include/rtps/entities/StatefulWriter.tpp | 7 ++++--- include/rtps/messages/MessageFactory.h | 2 +- include/rtps/messages/MessageTypes.h | 22 +++++++++++----------- 3 files changed, 16 insertions(+), 15 deletions(-) diff --git a/include/rtps/entities/StatefulWriter.tpp b/include/rtps/entities/StatefulWriter.tpp index da333239..aabff6af 100644 --- a/include/rtps/entities/StatefulWriter.tpp +++ b/include/rtps/entities/StatefulWriter.tpp @@ -347,6 +347,7 @@ bool StatefulWriterT::sendData( info.destAddr = locator.getIp4Address(); info.destPort = (Ip4Port_t)locator.port; + { Lock lock(m_mutex); const CacheChange *next = m_history.getChangeBySN(snMissing); @@ -358,9 +359,9 @@ bool StatefulWriterT::sendData( return false; } bool inlineQoS = false; - if(m_attributes.ownership_Kind == OwnershipKind_t::EXCLUSIVE){ - inlineQoS = true; - } + // if(m_attributes.ownership_Kind == OwnershipKind_t::EXCLUSIVE){ + // inlineQoS = true; + //} MessageFactory::addSubMessageData( info.buffer, next->data, inlineQoS, next->sequenceNumber, m_attributes.endpointGuid.entityId, reader.remoteReaderGuid.entityId, m_attributes.ownership_Kind, m_attributes.ownership_strenght diff --git a/include/rtps/messages/MessageFactory.h b/include/rtps/messages/MessageFactory.h index f253d350..4b1c64b6 100644 --- a/include/rtps/messages/MessageFactory.h +++ b/include/rtps/messages/MessageFactory.h @@ -32,7 +32,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include namespace rtps { - namespace MessageFactory { + namespace MessageFactory { const std::array PROTOCOL_TYPE{'R', 'T', 'P', 'S'}; const uint8_t numBytesUntilEndOfLength = 4; // The first bytes incl. submessagelength don't count diff --git a/include/rtps/messages/MessageTypes.h b/include/rtps/messages/MessageTypes.h index a465e17e..59861929 100644 --- a/include/rtps/messages/MessageTypes.h +++ b/include/rtps/messages/MessageTypes.h @@ -268,17 +268,17 @@ bool serializeMessage(Buffer &buffer, SubmessageData &msg) { sizeof(msg.writerSN.high)); buffer.append(reinterpret_cast(&msg.writerSN.low), sizeof(msg.writerSN.low)); - if(msg.ownershipKind == OwnershipKind_t::EXCLUSIVE){ - buffer.append(reinterpret_cast(SMElement::ParameterId::PID_OWNERSHIP_STRENGTH), sizeof(uint16_t)); - buffer.append(reinterpret_cast(sizeof(OwnershipStrength_t)), sizeof(uint16_t)); - buffer.append(reinterpret_cast(msg.ownershipStrength), sizeof(OwnershipStrength_t)); - buffer.append(reinterpret_cast(SMElement::ParameterId::PID_KEY_HASH), sizeof(uint16_t)); - uint8_t key[16]; - buffer.append(reinterpret_cast(16),sizeof(uint16_t)); - buffer.append(reinterpret_cast(key), 16); - - buffer.append(reinterpret_cast(SMElement::ParameterId::PID_SENTINEL), sizeof(uint16_t)); - buffer.append(reinterpret_cast(0), sizeof(uint16_t)); + if(msg.ownershipKind == OwnershipKind_t::EXCLUSIVE ){ + // buffer.append(reinterpret_cast(SMElement::ParameterId::PID_OWNERSHIP_STRENGTH), sizeof(uint16_t)); + // buffer.append(reinterpret_cast(sizeof(OwnershipStrength_t)), sizeof(uint16_t)); + // buffer.append(reinterpret_cast(msg.ownershipStrength), sizeof(OwnershipStrength_t)); + //buffer.append(reinterpret_cast(SMElement::ParameterId::PID_KEY_HASH), sizeof(uint16_t)); + //uint8_t key[16]; + // buffer.append(reinterpret_cast(16),sizeof(uint16_t)); + // buffer.append(reinterpret_cast(key), 16); + + // buffer.append(reinterpret_cast(SMElement::ParameterId::PID_SENTINEL), sizeof(uint16_t)); + // buffer.append(reinterpret_cast(0), sizeof(uint16_t)); } return true; From 602fd24de59d4f5f5a1ebd35814a68f34f8c5fca Mon Sep 17 00:00:00 2001 From: sven Date: Thu, 7 Oct 2021 01:28:02 +0200 Subject: [PATCH 17/25] reader handles ownership now --- include/rtps/common/types.h | 27 +++++++++--- include/rtps/entities/Reader.h | 3 +- include/rtps/entities/StatefulReader.h | 4 ++ include/rtps/entities/StatefulReader.tpp | 53 +++++++++++++++++++++++- include/rtps/entities/StatelessReader.h | 3 +- include/rtps/entities/WriterProxy.h | 6 +-- src/discovery/SEDPAgent.cpp | 2 +- src/entities/Participant.cpp | 19 +++++---- src/entities/StatelessReader.cpp | 6 +++ 9 files changed, 104 insertions(+), 19 deletions(-) diff --git a/include/rtps/common/types.h b/include/rtps/common/types.h index 2e7f368e..e9623b3f 100644 --- a/include/rtps/common/types.h +++ b/include/rtps/common/types.h @@ -30,7 +30,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include #include #include - +#include // TODO subnamespaces namespace rtps { @@ -40,9 +40,7 @@ typedef uint16_t Ip4Port_t; typedef uint16_t DataSize_t; typedef int8_t ParticipantId_t; // With UDP only 120 possible -class DynamicDataType{ - virtual void getKeyAttributes(uint8_t *buff, uint32_t &len) = 0;//Put all key attributes into an array -}; + enum class EntityKind_t : uint8_t { USER_DEFINED_UNKNOWN = 0x00, @@ -237,7 +235,18 @@ enum class ChangeForReaderStatusKind { enum class ChangeFromWriterStatusKind { LOST, MISSING, RECEIVED, UNKNOWN }; struct InstanceHandle_t { // TODO - uint64_t value; + //std::array key; + uint64_t key; + bool operator==(const InstanceHandle_t &other) const { + return key == other.key; + + //for (int i = 0; i < 16; i++) { + // if(key[i] != other.key[i]) { + // return false; + // } + //} + //return true; + } }; struct ParticipantMessageData { // TODO @@ -284,6 +293,14 @@ const SequenceNumber_t SEQUENCENUMBER_UNKNOWN = {-1, 0}; const Time_t TIME_ZERO = {}; const Time_t TIME_INVALID = {-1, 0xFFFFFFFF}; + +class WriterProxy; + +struct Instance_t{ + InstanceHandle_t handle; + WriterProxy *owner = nullptr; +}; + #ifndef CHIBIOS //has its own TIME_INFINITE const Time_t TIME_INFINITE = {0x7FFFFFFF, 0xFFFFFFFF}; #endif diff --git a/include/rtps/entities/Reader.h b/include/rtps/entities/Reader.h index 9dc43c33..caaf335d 100644 --- a/include/rtps/entities/Reader.h +++ b/include/rtps/entities/Reader.h @@ -76,12 +76,13 @@ class ReaderCacheChange { typedef void (*ddsReaderCallback_fp)(void *callee, const ReaderCacheChange &cacheChange); - +typedef void (*ddsGetKey_Callback_fp)(const uint8_t *data, uint32_t data_len, InstanceHandle_t &key); class Reader { public: TopicData m_attributes; virtual void newChange(const ReaderCacheChange &cacheChange) = 0; virtual void registerCallback(ddsReaderCallback_fp cb, void *callee) = 0; + virtual void registerKeyCallback(ddsGetKey_Callback_fp cb) = 0; virtual bool onNewHeartbeat(const SubmessageHeartbeat &msg, const GuidPrefix_t &remotePrefix) = 0; virtual bool addNewMatchedWriter(const WriterProxy &newProxy) = 0; diff --git a/include/rtps/entities/StatefulReader.h b/include/rtps/entities/StatefulReader.h index fe08eb15..b10b2d3e 100644 --- a/include/rtps/entities/StatefulReader.h +++ b/include/rtps/entities/StatefulReader.h @@ -41,6 +41,7 @@ template class StatefulReaderT final : public Reader { void init(const TopicData &attributes, NetworkDriver &driver); void newChange(const ReaderCacheChange &cacheChange) override; void registerCallback(ddsReaderCallback_fp cb, void *callee) override; + void registerKeyCallback(ddsGetKey_Callback_fp cb) override; bool addNewMatchedWriter(const WriterProxy &newProxy) override; void removeWriter(const Guid_t &guid) override; void removeWriterOfParticipant(const GuidPrefix_t &guidPrefix) override; @@ -53,8 +54,11 @@ template class StatefulReaderT final : public Reader { NetworkDriver *m_transport; TopicKind_t m_kind = TopicKind_t::NO_KEY; ddsReaderCallback_fp m_callback = nullptr; + ddsGetKey_Callback_fp m_KeyCallback = nullptr; void *m_callee = nullptr; sys_mutex_t m_mutex; + Guid_t searchOwner(InstanceHandle_t &handle, WriterProxy *proxy); + MemoryPool instances; }; using StatefulReader = StatefulReaderT; diff --git a/include/rtps/entities/StatefulReader.tpp b/include/rtps/entities/StatefulReader.tpp index a1c36d0d..05ad34cd 100644 --- a/include/rtps/entities/StatefulReader.tpp +++ b/include/rtps/entities/StatefulReader.tpp @@ -70,6 +70,38 @@ void StatefulReaderT::init(const TopicData &attributes, m_is_initialized_ = true; } +template +rtps::Guid_t StatefulReaderT::searchOwner(InstanceHandle_t &handle, WriterProxy *proxy){ + for(auto &instance : instances){ + if(instance.handle == handle){ + if(instance.owner == nullptr){ + instance.owner = proxy; + return proxy->remoteWriterGuid; + } + if(instance.owner == proxy){ + return proxy->remoteWriterGuid; + } + else{ + if(proxy->ownershipStrength < instance.owner->ownershipStrength){ + return instance.owner->remoteWriterGuid; + } + else if(proxy->ownershipStrength > instance.owner->ownershipStrength){ + instance.owner = proxy; + return proxy->remoteWriterGuid; + } + else{//equal strength , just pick the first one + return instance.owner->remoteWriterGuid; + } + } + } + } + Instance_t instance; + instance.owner = proxy; + instance.handle = handle; + instances.add(instance); + return proxy->remoteWriterGuid; +} + template void StatefulReaderT::newChange( const ReaderCacheChange &cacheChange) { @@ -77,11 +109,20 @@ void StatefulReaderT::newChange( return; } Lock lock{m_mutex}; + InstanceHandle_t handle; + for (auto &proxy : m_proxies) { if (proxy.remoteWriterGuid == cacheChange.writerGuid) { if (proxy.expectedSN == cacheChange.sn) { - m_callback(m_callee, cacheChange); ++proxy.expectedSN; + if(m_attributes.ownership_Kind == OwnershipKind_t::EXCLUSIVE) + m_KeyCallback(cacheChange.getData(),cacheChange.getDataSize(), handle); + if(searchOwner(handle, &proxy) == proxy.remoteWriterGuid) { // + m_callback(m_callee, cacheChange); + } + else{ + m_callback(m_callee, cacheChange); + } return; } } @@ -100,6 +141,16 @@ void StatefulReaderT::registerCallback(ddsReaderCallback_fp cb, } } +template +void StatefulReaderT::registerKeyCallback(ddsGetKey_Callback_fp cb){ + if(cb != nullptr){ + m_KeyCallback = cb; + } + else{ + SFR_LOG("Passed Key callback is nullptr\n"); + } +} + template bool StatefulReaderT::addNewMatchedWriter( const WriterProxy &newProxy) { diff --git a/include/rtps/entities/StatelessReader.h b/include/rtps/entities/StatelessReader.h index a67b99e4..d75eb718 100644 --- a/include/rtps/entities/StatelessReader.h +++ b/include/rtps/entities/StatelessReader.h @@ -39,9 +39,10 @@ class StatelessReader final : public Reader { bool addNewMatchedWriter(const WriterProxy &newProxy) override; void removeWriter(const Guid_t &guid) override; void removeWriterOfParticipant(const GuidPrefix_t &guidPrefix) override; - + void registerKeyCallback(ddsGetKey_Callback_fp cb) override; private: ddsReaderCallback_fp m_callback = nullptr; + ddsGetKey_Callback_fp m_keyCallback = nullptr; void *m_callee = nullptr; }; diff --git a/include/rtps/entities/WriterProxy.h b/include/rtps/entities/WriterProxy.h index a2302c0d..1a8e6719 100644 --- a/include/rtps/entities/WriterProxy.h +++ b/include/rtps/entities/WriterProxy.h @@ -35,13 +35,13 @@ struct WriterProxy { Count_t ackNackCount; Count_t hbCount; Locator remoteLocator; - + OwnershipStrength_t ownershipStrength; WriterProxy() = default; - WriterProxy(const Guid_t &guid, const Locator &loc) + WriterProxy(const Guid_t &guid, const Locator &loc, const OwnershipStrength_t ownershipStrength = 0) : remoteWriterGuid(guid), expectedSN(SequenceNumber_t{0, 1}), ackNackCount{1}, hbCount{0}, - remoteLocator(loc) {} + remoteLocator(loc), ownershipStrength(ownershipStrength) {} // For now, we don't store any packets, so we just request all starting from // the next expected diff --git a/src/discovery/SEDPAgent.cpp b/src/discovery/SEDPAgent.cpp index 99a3ded4..4eb43be4 100644 --- a/src/discovery/SEDPAgent.cpp +++ b/src/discovery/SEDPAgent.cpp @@ -133,7 +133,7 @@ void SEDPAgent::onNewPublisher(const TopicData &writerData) { SEDP_LOG("publisher\n"); #endif reader->addNewMatchedWriter( - WriterProxy{writerData.endpointGuid, writerData.unicastLocator}); + WriterProxy{writerData.endpointGuid, writerData.unicastLocator, writerData.ownership_strenght}); if (mfp_onNewPublisherCallback != nullptr) { mfp_onNewPublisherCallback(m_onNewPublisherArgs); } diff --git a/src/entities/Participant.cpp b/src/entities/Participant.cpp index d9ed43c3..456db065 100644 --- a/src/entities/Participant.cpp +++ b/src/entities/Participant.cpp @@ -164,10 +164,12 @@ rtps::Reader *Participant::getReaderByWriterId(const Guid_t &guid) const { rtps::Writer * Participant::getMatchingWriter(const TopicData &readerTopicData) const { for (uint8_t i = 0; i < m_numWriters; ++i) { - if (m_writers[i]->m_attributes.matchesTopicOf(readerTopicData) && - (readerTopicData.reliabilityKind == ReliabilityKind_t::BEST_EFFORT || - m_writers[i]->m_attributes.reliabilityKind == ReliabilityKind_t::RELIABLE || - m_writers[i]->m_attributes.ownership_Kind == readerTopicData.ownership_Kind)) { + bool topic = m_writers[i]->m_attributes.matchesTopicOf(readerTopicData); + bool reliable = (readerTopicData.reliabilityKind == ReliabilityKind_t::BEST_EFFORT || + m_writers[i]->m_attributes.reliabilityKind == ReliabilityKind_t::RELIABLE ); + + bool ownership = m_writers[i]->m_attributes.ownership_Kind == readerTopicData.ownership_Kind; + if(reliable && ownership && topic){ return m_writers[i]; } } @@ -177,10 +179,13 @@ Participant::getMatchingWriter(const TopicData &readerTopicData) const { rtps::Reader * Participant::getMatchingReader(const TopicData &writerTopicData) const { for (uint8_t i = 0; i < m_numReaders; ++i) { - if (m_readers[i]->m_attributes.matchesTopicOf(writerTopicData) && - (writerTopicData.reliabilityKind == ReliabilityKind_t::RELIABLE || + bool topic = m_readers[i]->m_attributes.matchesTopicOf(writerTopicData); + bool reliabale = (writerTopicData.reliabilityKind == ReliabilityKind_t::RELIABLE || m_readers[i]->m_attributes.reliabilityKind == - ReliabilityKind_t::BEST_EFFORT)) { + ReliabilityKind_t::BEST_EFFORT); + + bool ownership = m_readers[i]->m_attributes.ownership_Kind == writerTopicData.ownership_Kind; + if(ownership && reliabale && topic){ return m_readers[i]; } } diff --git a/src/entities/StatelessReader.cpp b/src/entities/StatelessReader.cpp index 2cd3d5b2..5e0195b8 100644 --- a/src/entities/StatelessReader.cpp +++ b/src/entities/StatelessReader.cpp @@ -94,4 +94,10 @@ bool StatelessReader::onNewHeartbeat(const SubmessageHeartbeat &, return true; } +void StatelessReader::registerKeyCallback(ddsGetKey_Callback_fp cb){ + if(cb != nullptr){ + m_keyCallback = cb; + } +} + #undef SLR_VERBOSE From 5bb8a80cef49ec24bbe26b8c8166248c15175f45 Mon Sep 17 00:00:00 2001 From: sven Date: Thu, 7 Oct 2021 14:07:08 +0200 Subject: [PATCH 18/25] ownership loop back, seems to not work 100% --- include/rtps/entities/StatefulReader.h | 2 +- include/rtps/entities/StatefulReader.tpp | 33 ++++++++++++++++++------ src/messages/MessageReceiver.cpp | 3 +++ 3 files changed, 29 insertions(+), 9 deletions(-) diff --git a/include/rtps/entities/StatefulReader.h b/include/rtps/entities/StatefulReader.h index b10b2d3e..b0ef0123 100644 --- a/include/rtps/entities/StatefulReader.h +++ b/include/rtps/entities/StatefulReader.h @@ -57,7 +57,7 @@ template class StatefulReaderT final : public Reader { ddsGetKey_Callback_fp m_KeyCallback = nullptr; void *m_callee = nullptr; sys_mutex_t m_mutex; - Guid_t searchOwner(InstanceHandle_t &handle, WriterProxy *proxy); + bool isOwner(InstanceHandle_t &handle, WriterProxy *proxy); MemoryPool instances; }; diff --git a/include/rtps/entities/StatefulReader.tpp b/include/rtps/entities/StatefulReader.tpp index 05ad34cd..9aceba8c 100644 --- a/include/rtps/entities/StatefulReader.tpp +++ b/include/rtps/entities/StatefulReader.tpp @@ -71,26 +71,26 @@ void StatefulReaderT::init(const TopicData &attributes, } template -rtps::Guid_t StatefulReaderT::searchOwner(InstanceHandle_t &handle, WriterProxy *proxy){ +bool StatefulReaderT::isOwner(InstanceHandle_t &handle, WriterProxy *proxy){ for(auto &instance : instances){ if(instance.handle == handle){ if(instance.owner == nullptr){ instance.owner = proxy; - return proxy->remoteWriterGuid; + return true; } if(instance.owner == proxy){ - return proxy->remoteWriterGuid; + return true; } else{ if(proxy->ownershipStrength < instance.owner->ownershipStrength){ - return instance.owner->remoteWriterGuid; + return false; } else if(proxy->ownershipStrength > instance.owner->ownershipStrength){ instance.owner = proxy; - return proxy->remoteWriterGuid; + return true; } else{//equal strength , just pick the first one - return instance.owner->remoteWriterGuid; + return false; } } } @@ -99,7 +99,7 @@ rtps::Guid_t StatefulReaderT::searchOwner(InstanceHandle_t &handl instance.owner = proxy; instance.handle = handle; instances.add(instance); - return proxy->remoteWriterGuid; + return true; } template @@ -117,7 +117,7 @@ void StatefulReaderT::newChange( ++proxy.expectedSN; if(m_attributes.ownership_Kind == OwnershipKind_t::EXCLUSIVE) m_KeyCallback(cacheChange.getData(),cacheChange.getDataSize(), handle); - if(searchOwner(handle, &proxy) == proxy.remoteWriterGuid) { // + if(isOwner(handle, &proxy)) { // m_callback(m_callee, cacheChange); } else{ @@ -164,6 +164,14 @@ bool StatefulReaderT::addNewMatchedWriter( template void StatefulReaderT::removeWriter(const Guid_t &guid) { + auto isElementToRemove_Instance = [&](const Instance_t &instance){ + return instance.owner->remoteWriterGuid == guid; + }; + auto thunk_instance = [](void *arg, const Instance_t &value) { + return (*static_cast(arg))(value); + }; + instances.remove(thunk_instance, &isElementToRemove_Instance); + auto isElementToRemove = [&](const WriterProxy &proxy) { return proxy.remoteWriterGuid == guid; }; @@ -172,11 +180,20 @@ void StatefulReaderT::removeWriter(const Guid_t &guid) { }; m_proxies.remove(thunk, &isElementToRemove); + } template void StatefulReaderT::removeWriterOfParticipant( const GuidPrefix_t &guidPrefix) { + auto isElementToRemove_Instance = [&](const Instance_t &instance){ + return instance.owner->remoteWriterGuid.prefix == guidPrefix; + }; + auto thunk_instance = [](void *arg, const Instance_t &value) { + return (*static_cast(arg))(value); + }; + instances.remove(thunk_instance, &isElementToRemove_Instance); + auto isElementToRemove = [&](const WriterProxy &proxy) { return proxy.remoteWriterGuid.prefix == guidPrefix; }; diff --git a/src/messages/MessageReceiver.cpp b/src/messages/MessageReceiver.cpp index 849f1a95..0055faab 100644 --- a/src/messages/MessageReceiver.cpp +++ b/src/messages/MessageReceiver.cpp @@ -115,6 +115,9 @@ bool MessageReceiver::processSubmessage(MessageProcessingInfo &msgInfo, RECV_LOG("Info_TS submessage not relevant.\n"); success = true; // Not relevant now break; + case SubmessageKind::GAP: + //success = processGapSubmessage(&msgInfo, ) + break; default: RECV_LOG("Submessage of type %u currently not supported. Skipping..\n", static_cast(submsgHeader.submessageId)); From 0cd92f40a1bd13f5088146f7b3a8775aecfd9090 Mon Sep 17 00:00:00 2001 From: sven Date: Thu, 7 Oct 2021 14:14:35 +0200 Subject: [PATCH 19/25] found ownership error --- include/rtps/entities/StatefulReader.tpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/include/rtps/entities/StatefulReader.tpp b/include/rtps/entities/StatefulReader.tpp index 9aceba8c..e2e49c38 100644 --- a/include/rtps/entities/StatefulReader.tpp +++ b/include/rtps/entities/StatefulReader.tpp @@ -115,11 +115,12 @@ void StatefulReaderT::newChange( if (proxy.remoteWriterGuid == cacheChange.writerGuid) { if (proxy.expectedSN == cacheChange.sn) { ++proxy.expectedSN; - if(m_attributes.ownership_Kind == OwnershipKind_t::EXCLUSIVE) - m_KeyCallback(cacheChange.getData(),cacheChange.getDataSize(), handle); - if(isOwner(handle, &proxy)) { // + if(m_attributes.ownership_Kind == OwnershipKind_t::EXCLUSIVE) { + m_KeyCallback(cacheChange.getData(), cacheChange.getDataSize(), handle); + if (isOwner(handle, &proxy)) { // m_callback(m_callee, cacheChange); } + } else{ m_callback(m_callee, cacheChange); } From db468746555bfee770b7d3bc65c2147247e4cad7 Mon Sep 17 00:00:00 2001 From: sven Date: Tue, 12 Oct 2021 22:14:30 +0200 Subject: [PATCH 20/25] best effort reader ownership --- include/rtps/common/types.h | 2 +- include/rtps/entities/StatefulReader.h | 2 +- include/rtps/entities/StatefulReader.tpp | 4 +-- include/rtps/entities/StatelessReader.h | 3 ++ src/entities/StatelessReader.cpp | 43 ++++++++++++++++++++++++ 5 files changed, 50 insertions(+), 4 deletions(-) diff --git a/include/rtps/common/types.h b/include/rtps/common/types.h index e9623b3f..264f7b5f 100644 --- a/include/rtps/common/types.h +++ b/include/rtps/common/types.h @@ -293,7 +293,7 @@ const SequenceNumber_t SEQUENCENUMBER_UNKNOWN = {-1, 0}; const Time_t TIME_ZERO = {}; const Time_t TIME_INVALID = {-1, 0xFFFFFFFF}; - +const Time_t TIME_INFINITY = {0x7FFFFFFF, 0xFFFFFFFF}; class WriterProxy; struct Instance_t{ diff --git a/include/rtps/entities/StatefulReader.h b/include/rtps/entities/StatefulReader.h index b0ef0123..47838094 100644 --- a/include/rtps/entities/StatefulReader.h +++ b/include/rtps/entities/StatefulReader.h @@ -58,7 +58,7 @@ template class StatefulReaderT final : public Reader { void *m_callee = nullptr; sys_mutex_t m_mutex; bool isOwner(InstanceHandle_t &handle, WriterProxy *proxy); - MemoryPool instances; + MemoryPool m_instances; }; using StatefulReader = StatefulReaderT; diff --git a/include/rtps/entities/StatefulReader.tpp b/include/rtps/entities/StatefulReader.tpp index e2e49c38..62bde72c 100644 --- a/include/rtps/entities/StatefulReader.tpp +++ b/include/rtps/entities/StatefulReader.tpp @@ -72,7 +72,7 @@ void StatefulReaderT::init(const TopicData &attributes, template bool StatefulReaderT::isOwner(InstanceHandle_t &handle, WriterProxy *proxy){ - for(auto &instance : instances){ + for(auto &instance : m_instances){ if(instance.handle == handle){ if(instance.owner == nullptr){ instance.owner = proxy; @@ -98,7 +98,7 @@ bool StatefulReaderT::isOwner(InstanceHandle_t &handle, WriterPro Instance_t instance; instance.owner = proxy; instance.handle = handle; - instances.add(instance); + m_instances.add(instance); return true; } diff --git a/include/rtps/entities/StatelessReader.h b/include/rtps/entities/StatelessReader.h index d75eb718..0a4eeaa8 100644 --- a/include/rtps/entities/StatelessReader.h +++ b/include/rtps/entities/StatelessReader.h @@ -44,6 +44,9 @@ class StatelessReader final : public Reader { ddsReaderCallback_fp m_callback = nullptr; ddsGetKey_Callback_fp m_keyCallback = nullptr; void *m_callee = nullptr; + MemoryPool m_instances; + + bool isOwner(InstanceHandle_t &handle, WriterProxy *proxy); }; } // namespace rtps diff --git a/src/entities/StatelessReader.cpp b/src/entities/StatelessReader.cpp index 5e0195b8..18fd76ae 100644 --- a/src/entities/StatelessReader.cpp +++ b/src/entities/StatelessReader.cpp @@ -44,8 +44,51 @@ void StatelessReader::init(const TopicData &attributes) { m_is_initialized_ = true; } +bool StatelessReader::isOwner(InstanceHandle_t &handle, WriterProxy *proxy){ + for(auto &instance : m_instances){ + if(instance.handle == handle){ + if(instance.owner == nullptr){ + instance.owner = proxy; + return true; + } + if(instance.owner == proxy){ + return true; + } + else{ + if(proxy->ownershipStrength < instance.owner->ownershipStrength){ + return false; + } + else if(proxy->ownershipStrength > instance.owner->ownershipStrength){ + instance.owner = proxy; + return true; + } + else{//equal strength , just pick the first one + return false; + } + } + } + } + Instance_t instance; + instance.owner = proxy; + instance.handle = handle; + m_instances.add(instance); + return true; +} + void StatelessReader::newChange(const ReaderCacheChange &cacheChange) { if (m_callback != nullptr) { + if(m_attributes.ownership_Kind == OwnershipKind_t::EXCLUSIVE) { + InstanceHandle_t handle; + m_keyCallback(cacheChange.getData(), cacheChange.getDataSize(), handle); + for (auto &proxy:m_proxies) { + if (cacheChange.writerGuid == proxy.remoteWriterGuid && isOwner(handle, &proxy)) { // + m_callback(m_callee, cacheChange); + } + } + } + else{ + m_callback(m_callee, cacheChange); + } m_callback(m_callee, cacheChange); } } From 567947dfc5f3e32432fa33ffeffe41533a1e8181 Mon Sep 17 00:00:00 2001 From: sven Date: Tue, 12 Oct 2021 22:23:01 +0200 Subject: [PATCH 21/25] best effort reader ownership --- include/rtps/entities/Domain.h | 3 --- src/entities/Domain.cpp | 23 +++++++++-------------- 2 files changed, 9 insertions(+), 17 deletions(-) diff --git a/include/rtps/entities/Domain.h b/include/rtps/entities/Domain.h index 18f95a0b..150d13ca 100644 --- a/include/rtps/entities/Domain.h +++ b/include/rtps/entities/Domain.h @@ -50,9 +50,6 @@ class Domain { const char *typeName, bool reliable, bool enforceUnicast = false); - Writer *createWriter(Participant &part, const char *topicName, - const char *typeName, OwnershipStrength_t ownership_strenght, - bool enforceUnicast = false); Writer *createWriter(Participant &part, const char *topicName, const char *typeName, OwnershipKind_t ownership_kind, OwnershipStrength_t ownershipStrength, bool reliable , bool topichasKey , bool enforceUnicast); diff --git a/src/entities/Domain.cpp b/src/entities/Domain.cpp index d7ec5cd0..eccdc80d 100644 --- a/src/entities/Domain.cpp +++ b/src/entities/Domain.cpp @@ -313,7 +313,7 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, con bool topichasKey, bool enforceUnicast ){ // Check if there is enough capacity for more writers - if (((reliable || ownership_kind == OwnershipKind_t::EXCLUSIVE) && m_statefulWriters.size() <= m_numStatefulWriters) || //Ownership needs reliable writer + if ((reliable && m_statefulWriters.size() <= m_numStatefulWriters) || ((!reliable) && m_statelessWriters.size() <= m_numStatelessWriters) || part.isWritersFull()) { DOMAIN_LOG("No Writer created. Max Number of Writers reached.\n"); @@ -341,12 +341,12 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, con attributes.unicastLocator = getUserUnicastLocator(part.m_participantId); attributes.durabilityKind = DurabilityKind_t::TRANSIENT_LOCAL; + attributes.ownership_Kind = ownership_kind; + if(ownership_kind == OwnershipKind_t::EXCLUSIVE) { + attributes.ownership_strenght = ownershipStrength; + } if (reliable || ownership_kind == OwnershipKind_t::EXCLUSIVE) { attributes.reliabilityKind = ReliabilityKind_t::RELIABLE; - if(ownership_kind == OwnershipKind_t::EXCLUSIVE){ - attributes.ownership_Kind = OwnershipKind_t::EXCLUSIVE; - attributes.ownership_strenght = ownershipStrength; - } StatefulWriter &writer = m_statefulWriters[m_numStatefulWriters++]; writer.init(attributes, kind, &m_threadPool, m_transport, enforceUnicast); @@ -365,12 +365,6 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, con } } -rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, - const char *typeName, OwnershipStrength_t ownership_strenght, - bool enforceUnicast ){ - return createWriter(part, topicName,typeName, OwnershipKind_t::EXCLUSIVE, ownership_strenght, true, true, enforceUnicast); -} - rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, const char *typeName, bool reliable, bool enforceUnicast) { @@ -380,7 +374,7 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, rtps::Reader *Domain::createReader(Participant &part, const char *topicName, bool topichasKey, const char *typeName, bool reliable, OwnershipKind_t ownershipKind, ip4_addr_t mcastaddress){ - if (((reliable || (ownershipKind == OwnershipKind_t::EXCLUSIVE) )&& m_statefulReaders.size() <= m_numStatefulReaders) || + if ((reliable&& m_statefulReaders.size() <= m_numStatefulReaders) || (!reliable && m_statelessReaders.size() <= m_numStatelessReaders) || part.isReadersFull()) { @@ -426,13 +420,14 @@ rtps::Reader *Domain::createReader(Participant &part, const char *topicName, boo } DOMAIN_LOG("Creating reader[%s, %s]\n", topicName, typeName); - if (reliable || (ownershipKind == OwnershipKind_t::EXCLUSIVE)) { + attributes.ownership_Kind = ownershipKind; + + if (reliable) { if (m_numStatefulReaders == m_statefulReaders.size()) { return nullptr; } attributes.reliabilityKind = ReliabilityKind_t::RELIABLE; - attributes.ownership_Kind = ownershipKind; StatefulReader &reader = m_statefulReaders[m_numStatefulReaders++]; reader.init(attributes, m_transport); From fa48b75ea5b2a731ab12b54f7a8a025615602f16 Mon Sep 17 00:00:00 2001 From: sven Date: Sun, 17 Oct 2021 14:51:10 +0200 Subject: [PATCH 22/25] solved cdr alignment issue --- include/rtps/common/MD5.h | 23 ++ include/rtps/common/types.h | 16 +- include/rtps/entities/StatefulReader.tpp | 4 +- src/common/MD5.cpp | 500 +++++++++++++++++++++++ src/entities/StatelessReader.cpp | 15 + 5 files changed, 548 insertions(+), 10 deletions(-) create mode 100644 include/rtps/common/MD5.h create mode 100644 src/common/MD5.cpp diff --git a/include/rtps/common/MD5.h b/include/rtps/common/MD5.h new file mode 100644 index 00000000..27408046 --- /dev/null +++ b/include/rtps/common/MD5.h @@ -0,0 +1,23 @@ +// +// Created by Sven on 14.10.21. +// + +#ifndef RTPS_MD5_H +#define RTPS_MD5_H + +#include "rtps/config.h" + +namespace rtps{ +// MD5 context. + typedef struct { + uint32_t state[4]; // state (ABCD) + uint32_t count[2]; // number of bits, modulo 2^64 (lsb first) + uint8_t buffer[64]; // input buffer + } MD5_CTX; + + void MD5Init (MD5_CTX *); + void MD5Update (MD5_CTX *, const uint8_t *, uint32_t); + void MD5Final (uint8_t[16], MD5_CTX *); + +} +#endif //EMBEDDEDRTPS_EXAMPLE_MD5_H diff --git a/include/rtps/common/types.h b/include/rtps/common/types.h index 264f7b5f..27bd2f3a 100644 --- a/include/rtps/common/types.h +++ b/include/rtps/common/types.h @@ -235,17 +235,17 @@ enum class ChangeForReaderStatusKind { enum class ChangeFromWriterStatusKind { LOST, MISSING, RECEIVED, UNKNOWN }; struct InstanceHandle_t { // TODO - //std::array key; - uint64_t key; + uint8_t key[16]; + //uint64_t key; bool operator==(const InstanceHandle_t &other) const { return key == other.key; - //for (int i = 0; i < 16; i++) { - // if(key[i] != other.key[i]) { - // return false; - // } - //} - //return true; + for (int i = 0; i < 16; i++) { + if(key[i] != other.key[i]) { + return false; + } + } + return true; } }; diff --git a/include/rtps/entities/StatefulReader.tpp b/include/rtps/entities/StatefulReader.tpp index 62bde72c..6ddc510d 100644 --- a/include/rtps/entities/StatefulReader.tpp +++ b/include/rtps/entities/StatefulReader.tpp @@ -171,7 +171,7 @@ void StatefulReaderT::removeWriter(const Guid_t &guid) { auto thunk_instance = [](void *arg, const Instance_t &value) { return (*static_cast(arg))(value); }; - instances.remove(thunk_instance, &isElementToRemove_Instance); + m_instances.remove(thunk_instance, &isElementToRemove_Instance); auto isElementToRemove = [&](const WriterProxy &proxy) { return proxy.remoteWriterGuid == guid; @@ -193,7 +193,7 @@ void StatefulReaderT::removeWriterOfParticipant( auto thunk_instance = [](void *arg, const Instance_t &value) { return (*static_cast(arg))(value); }; - instances.remove(thunk_instance, &isElementToRemove_Instance); + m_instances.remove(thunk_instance, &isElementToRemove_Instance); auto isElementToRemove = [&](const WriterProxy &proxy) { return proxy.remoteWriterGuid.prefix == guidPrefix; diff --git a/src/common/MD5.cpp b/src/common/MD5.cpp new file mode 100644 index 00000000..fc645e4c --- /dev/null +++ b/src/common/MD5.cpp @@ -0,0 +1,500 @@ +// +// Created by Sven on 14.10.21. +// + +#include + +#include +//#include "rtps/commonmd5.h" +using namespace rtps; +// Constants for MD5Transform routine. + +#define MD5_memcpy memcpy +#define MD5_memset memset + +#define S11 7 +#define S12 12 +#define S13 17 +#define S14 22 +#define S21 5 +#define S22 9 +#define S23 14 +#define S24 20 +#define S31 4 +#define S32 11 +#define S33 16 +#define S34 23 +#define S41 6 +#define S42 10 +#define S43 15 +#define S44 21 + +static const uint8_t PADDING[64] = { + 0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 +}; + +// F, G, H and I are basic MD5 functions. + +#define F(x, y, z) (((x) & (y)) | ((~x) & (z))) +#define G(x, y, z) (((x) & (z)) | ((y) & (~z))) +#define H(x, y, z) ((x) ^ (y) ^ (z)) +#define I(x, y, z) ((y) ^ ((x) | (~z))) + +// ROTATE_LEFT rotates x left n bits. + +#define ROTATE_LEFT(x, n) (((x) << (n)) | ((x) >> (32-(n)))) +//extern DWORD fast_long_roll( DWORD l, signed char n ) small; +//#define ROTATE_LEFT fast_long_roll + +// FF, GG, HH, and II transformations for rounds 1, 2, 3, and 4. +// Rotation is separate from addition to prevent recomputation. + +#define FF(a, b, c, d, x, s, ac) { \ + (a) += F ((b), (c), (d)) + (x) + (uint32_t)(ac); \ + (a) = ROTATE_LEFT ((a), (s)); \ + (a) += (b); \ + } +#define GG(a, b, c, d, x, s, ac) { \ + (a) += G ((b), (c), (d)) + (x) + (uint32_t)(ac); \ + (a) = ROTATE_LEFT ((a), (s)); \ + (a) += (b); \ + } +#define HH(a, b, c, d, x, s, ac) { \ + (a) += H ((b), (c), (d)) + (x) + (uint32_t)(ac); \ + (a) = ROTATE_LEFT ((a), (s)); \ + (a) += (b); \ + } +#define II(a, b, c, d, x, s, ac) { \ + (a) += I ((b), (c), (d)) + (x) + (uint32_t)(ac); \ + (a) = ROTATE_LEFT ((a), (s)); \ + (a) += (b); \ + } + +// Encodes input (UINT4) into output (BYTE). Assumes len is +// a multiple of 4. +static void Encode (uint8_t *output, const uint32_t *input, uint16_t len ) +{ + uint16_t i, j; + + for (i = 0, j = 0; j < len; i++, j += 4) + { + output[j] = (uint8_t)(input[i] & 0xff); + *(output+j+1) = (uint8_t)((input[i] >> 8) & 0xff); + *(output+j+2) = (uint8_t)((input[i] >> 16) & 0xff); + *(output+j+3) = (uint8_t)((input[i] >> 24) & 0xff); + } +} + +// Decodes input (BYTE) into output (UINT4). Assumes len is +// multiple of 4. +static void Decode (uint32_t *output, const uint8_t *input, uint16_t len ) +{ + uint16_t i, j; + + for (i = 0, j = 0; j < len; i++, j += 4) + output[i] = ((uint32_t)input[j]) | (((uint32_t)*(input + j + 1)) << 8) | (((uint32_t)*(input + j + 2)) << 16) | (((uint32_t)*(input + j + 3)) << 24); +} + +// Note: Replace "for loop" with standard memcpy if possible. + +//static void MD5_memcpy (char * output, char * input, WORD len ) +//{ +//// WORD i; +// +// memcpy(output,input,len); +//// for (i = 0; i < len; i++) +//// output[i] = input[i]; +//} +// +//// Note: Replace "for loop" with standard memset if possible. +//static void MD5_memset (char * output, char value, WORD len ) +//{ +//// WORD i; +// +// memset( output, value, len ); +//// for (i = 0; i < len; i++) +//// ((char *)output)[i] = (char)value; +//} + +// MD5 basic transformation. Transforms state based on block. + +static void MD5Transform (uint32_t state[4], const uint8_t block[64]) +{ + uint32_t a = state[0], b = state[1], c = state[2], d = state[3], x[16]; + + Decode (x, block, 64); + + // Round 1 + FF (a, b, c, d, x[ 0], S11, 0xd76aa478); // 1 + FF (d, a, b, c, x[ 1], S12, 0xe8c7b756); // 2 + FF (c, d, a, b, x[ 2], S13, 0x242070db); // 3 + FF (b, c, d, a, x[ 3], S14, 0xc1bdceee); // 4 + FF (a, b, c, d, x[ 4], S11, 0xf57c0faf); // 5 + FF (d, a, b, c, x[ 5], S12, 0x4787c62a); // 6 + FF (c, d, a, b, x[ 6], S13, 0xa8304613); // 7 + FF (b, c, d, a, x[ 7], S14, 0xfd469501); // 8 + FF (a, b, c, d, x[ 8], S11, 0x698098d8); // 9 + FF (d, a, b, c, x[ 9], S12, 0x8b44f7af); // 10 + FF (c, d, a, b, x[10], S13, 0xffff5bb1); // 11 + FF (b, c, d, a, x[11], S14, 0x895cd7be); // 12 + FF (a, b, c, d, x[12], S11, 0x6b901122); // 13 + FF (d, a, b, c, x[13], S12, 0xfd987193); // 14 + FF (c, d, a, b, x[14], S13, 0xa679438e); // 15 + FF (b, c, d, a, x[15], S14, 0x49b40821); // 16 + + // Round 2 + GG (a, b, c, d, x[ 1], S21, 0xf61e2562); // 17 + GG (d, a, b, c, x[ 6], S22, 0xc040b340); // 18 + GG (c, d, a, b, x[11], S23, 0x265e5a51); // 19 + GG (b, c, d, a, x[ 0], S24, 0xe9b6c7aa); // 20 + GG (a, b, c, d, x[ 5], S21, 0xd62f105d); // 21 + GG (d, a, b, c, x[10], S22, 0x2441453); // 22 + GG (c, d, a, b, x[15], S23, 0xd8a1e681); // 23 + GG (b, c, d, a, x[ 4], S24, 0xe7d3fbc8); // 24 + GG (a, b, c, d, x[ 9], S21, 0x21e1cde6); // 25 + GG (d, a, b, c, x[14], S22, 0xc33707d6); // 26 + GG (c, d, a, b, x[ 3], S23, 0xf4d50d87); // 27 + GG (b, c, d, a, x[ 8], S24, 0x455a14ed); // 28 + GG (a, b, c, d, x[13], S21, 0xa9e3e905); // 29 + GG (d, a, b, c, x[ 2], S22, 0xfcefa3f8); // 30 + GG (c, d, a, b, x[ 7], S23, 0x676f02d9); // 31 + GG (b, c, d, a, x[12], S24, 0x8d2a4c8a); // 32 + + // Round 3 + HH (a, b, c, d, x[ 5], S31, 0xfffa3942); // 33 + HH (d, a, b, c, x[ 8], S32, 0x8771f681); // 34 + HH (c, d, a, b, x[11], S33, 0x6d9d6122); // 35 + HH (b, c, d, a, x[14], S34, 0xfde5380c); // 36 + HH (a, b, c, d, x[ 1], S31, 0xa4beea44); // 37 + HH (d, a, b, c, x[ 4], S32, 0x4bdecfa9); // 38 + HH (c, d, a, b, x[ 7], S33, 0xf6bb4b60); // 39 + HH (b, c, d, a, x[10], S34, 0xbebfbc70); // 40 + HH (a, b, c, d, x[13], S31, 0x289b7ec6); // 41 + HH (d, a, b, c, x[ 0], S32, 0xeaa127fa); // 42 + HH (c, d, a, b, x[ 3], S33, 0xd4ef3085); // 43 + HH (b, c, d, a, x[ 6], S34, 0x4881d05); // 44 + HH (a, b, c, d, x[ 9], S31, 0xd9d4d039); // 45 + HH (d, a, b, c, x[12], S32, 0xe6db99e5); // 46 + HH (c, d, a, b, x[15], S33, 0x1fa27cf8); // 47 + HH (b, c, d, a, x[ 2], S34, 0xc4ac5665); // 48 + + // Round 4 + II (a, b, c, d, x[ 0], S41, 0xf4292244); // 49 + II (d, a, b, c, x[ 7], S42, 0x432aff97); // 50 + II (c, d, a, b, x[14], S43, 0xab9423a7); // 51 + II (b, c, d, a, x[ 5], S44, 0xfc93a039); // 52 + II (a, b, c, d, x[12], S41, 0x655b59c3); // 53 + II (d, a, b, c, x[ 3], S42, 0x8f0ccc92); // 54 + II (c, d, a, b, x[10], S43, 0xffeff47d); // 55 + II (b, c, d, a, x[ 1], S44, 0x85845dd1); // 56 + II (a, b, c, d, x[ 8], S41, 0x6fa87e4f); // 57 + II (d, a, b, c, x[15], S42, 0xfe2ce6e0); // 58 + II (c, d, a, b, x[ 6], S43, 0xa3014314); // 59 + II (b, c, d, a, x[13], S44, 0x4e0811a1); // 60 + II (a, b, c, d, x[ 4], S41, 0xf7537e82); // 61 + II (d, a, b, c, x[11], S42, 0xbd3af235); // 62 + II (c, d, a, b, x[ 2], S43, 0x2ad7d2bb); // 63 + II (b, c, d, a, x[ 9], S44, 0xeb86d391); // 64 + + state[0] += a; + state[1] += b; + state[2] += c; + state[3] += d; + + // Zeroize sensitive information. + MD5_memset ((char *)x, 0, sizeof (x)); +} + +void rtps::MD5Init (MD5_CTX *context) +{ + context->count[0] = context->count[1] = 0; + // Load magic initialization constants. + + context->state[0] = 0x67452301; + context->state[1] = 0xefcdab89; + context->state[2] = 0x98badcfe; + context->state[3] = 0x10325476; +} + +// MD5 block update operation. Continues an MD5 message-digest +// operation, processing another message block, and updating the +// context. + +void rtps::MD5Update ( MD5_CTX *context, const uint8_t *input, uint32_t inputLen ) +{ + uint16_t i, index, partLen; + + // Compute number of bytes mod 64 + index = (uint16_t)((context->count[0] >> 3) & 0x3F); + + // Update number of bits + if ((context->count[0] += (inputLen << 3)) < (inputLen << 3)) + context->count[1]++; + + context->count[1] += (inputLen >> 29); + + partLen = 64 - index; + + // Transform as many times as possible. + + if (inputLen >= partLen) + { + MD5_memcpy(&context->buffer[index], (char *)input, (int)partLen); + MD5Transform (context->state, context->buffer); + + for (i = partLen; i + 63 < inputLen; i += 64) + MD5Transform (context->state, &input[i]); + + index = 0; + } + else + i = 0; + + // Buffer remaining input + MD5_memcpy(&context->buffer[index], (char *)&input[i],(int)(inputLen-i)); +} + +// MD5 finalization. Ends an MD5 message-digest operation, writing the +// the message digest and zeroizing the context. +void rtps::MD5Final ( uint8_t digest[16], MD5_CTX *context ) +{ + uint8_t bits[8]; + uint16_t index, padLen; + + // Save number of bits + Encode (bits, context->count, 8); + + // Pad out to 56 mod 64. + + index = (uint16_t)((context->count[0] >> 3) & 0x3f); + padLen = (index < 56) ? (56 - index) : (120 - index); + rtps::MD5Update (context, PADDING, (uint32_t)padLen); + + // Append length (before padding) + rtps::MD5Update (context, bits, 8L); + + // Store state in digest + Encode (digest, context->state, 16); + + // Zeroize sensitive information. + MD5_memset ((char *)context, 0, sizeof (*context)); +} + + + +/* + The MD5 algorithm is designed to be quite fast on 32-bit machines. In + addition, the MD5 algorithm does not require any large substitution + tables; the algorithm can be coded quite compactly. + The MD5 algorithm is an extension of the MD4 message-digest algorithm + 1,2]. MD5 is slightly slower than MD4, but is more "conservative" in + design. MD5 was designed because it was felt that MD4 was perhaps + being adopted for use more quickly than justified by the existing + critical review; because MD4 was designed to be exceptionally fast, + it is "at the edge" in terms of risking successful cryptanalytic + attack. MD5 backs off a bit, giving up a little in speed for a much + greater likelihood of ultimate security. It incorporates some + suggestions made by various reviewers, and contains additional + optimizations. The MD5 algorithm is being placed in the public domain + for review and possible adoption as a standard. + For OSI-based applications, MD5's object identifier is + md5 OBJECT IDENTIFIER ::= + iso(1) member-body(2) US(840) rsadsi(113549) digestAlgorithm(2) 5} + In the X.509 type AlgorithmIdentifier [3], the parameters for MD5 + should have type NULL. +2. Terminology and Notation + In this document a "word" is a 32-bit quantity and a "byte" is an + eight-bit quantity. A sequence of bits can be interpreted in a + natural manner as a sequence of bytes, where each consecutive group + of eight bits is interpreted as a byte with the high-order (most + significant) bit of each byte listed first. Similarly, a sequence of + bytes can be interpreted as a sequence of 32-bit words, where each + consecutive group of four bytes is interpreted as a word with the + low-order (least significant) byte given first. + Let x_i denote "x sub i". If the subscript is an expression, we + surround it in braces, as in x_{i+1}. Similarly, we use ^ for + superscripts (exponentiation), so that x^i denotes x to the i-th + power. + Let the symbol "+" denote addition of words (i.e., modulo-2^32 + addition). Let X <<< s denote the 32-bit value obtained by circularly + shifting (rotating) X left by s bit positions. Let not(X) denote the + bit-wise complement of X, and let X v Y denote the bit-wise OR of X + and Y. Let X xor Y denote the bit-wise XOR of X and Y, and let XY + denote the bit-wise AND of X and Y. +3. MD5 Algorithm Description + We begin by supposing that we have a b-bit message as input, and that + we wish to find its message digest. Here b is an arbitrary + nonnegative integer; b may be zero, it need not be a multiple of + eight, and it may be arbitrarily large. We imagine the bits of the + message written down as follows: + m_0 m_1 ... m_{b-1} + The following five steps are performed to compute the message digest + of the message. +3.1 Step 1. Append Padding Bits + The message is "padded" (extended) so that its length (in bits) is + congruent to 448, modulo 512. That is, the message is extended so + that it is just 64 bits shy of being a multiple of 512 bits long. + Padding is always performed, even if the length of the message is + already congruent to 448, modulo 512. + Padding is performed as follows: a single "1" bit is appended to the + message, and then "0" bits are appended so that the length in bits of + the padded message becomes congruent to 448, modulo 512. In all, at + least one bit and at most 512 bits are appended. +3.2 Step 2. Append Length + A 64-bit representation of b (the length of the message before the + padding bits were added) is appended to the result of the previous + step. In the unlikely event that b is greater than 2^64, then only + the low-order 64 bits of b are used. (These bits are appended as two + 32-bit words and appended low-order word first in accordance with the + previous conventions.) + At this point the resulting message (after padding with bits and with + b) has a length that is an exact multiple of 512 bits. Equivalently, + this message has a length that is an exact multiple of 16 (32-bit) + words. Let M[0 ... N-1] denote the words of the resulting message, + where N is a multiple of 16. +3.3 Step 3. Initialize MD Buffer + A four-word buffer (A,B,C,D) is used to compute the message digest. + Here each of A, B, C, D is a 32-bit register. These registers are + initialized to the following values in hexadecimal, low-order bytes + first): + word A: 01 23 45 67 + word B: 89 ab cd ef + word C: fe dc ba 98 + word D: 76 54 32 10 +3.4 Step 4. Process Message in 16-Word Blocks + We first define four auxiliary functions that each take as input + three 32-bit words and produce as output one 32-bit word. + F(X,Y,Z) = XY v not(X) Z + G(X,Y,Z) = XZ v Y not(Z) + H(X,Y,Z) = X xor Y xor Z + I(X,Y,Z) = Y xor (X v not(Z)) + In each bit position F acts as a conditional: if X then Y else Z. + The function F could have been defined using + instead of v since XY + and not(X)Z will never have 1's in the same bit position.) It is + interesting to note that if the bits of X, Y, and Z are independent + and unbiased, the each bit of F(X,Y,Z) will be independent and + unbiased. + The functions G, H, and I are similar to the function F, in that they + act in "bitwise parallel" to produce their output from the bits of X, + Y, and Z, in such a manner that if the corresponding bits of X, Y, + and Z are independent and unbiased, then each bit of G(X,Y,Z), + H(X,Y,Z), and I(X,Y,Z) will be independent and unbiased. Note that + the function H is the bit-wise "xor" or "parity" function of its + inputs. + This step uses a 64-element table T[1 ... 64] constructed from the + sine function. Let T[i] denote the i-th element of the table, which + is equal to the integer part of 4294967296 times abs(sin(i)), where i + is in radians. The elements of the table are given in the appendix. + Do the following: + // Process each 16-word block. + For i = 0 to N/16-1 do + // Copy block i into X. + For j = 0 to 15 do + Set X[j] to M[i*16+j]. + end // of loop on j + // Save A as AA, B as BB, C as CC, and D as DD. + AA = A + BB = B + CC = C + DD = D + // Round 1. + // Let [abcd k s i] denote the operation + // a = b + ((a + F(b,c,d) + X[k] + T[i]) <<< s). + // Do the following 16 operations. + [ABCD 0 7 1] [DABC 1 12 2] [CDAB 2 17 3] [BCDA 3 22 4] + [ABCD 4 7 5] [DABC 5 12 6] [CDAB 6 17 7] [BCDA 7 22 8] + [ABCD 8 7 9] [DABC 9 12 10] [CDAB 10 17 11] [BCDA 11 22 12] + [ABCD 12 7 13] [DABC 13 12 14] [CDAB 14 17 15] [BCDA 15 22 16] + // Round 2. + // Let [abcd k s i] denote the operation + // a = b + ((a + G(b,c,d) + X[k] + T[i]) <<< s). + // Do the following 16 operations. + [ABCD 1 5 17] [DABC 6 9 18] [CDAB 11 14 19] [BCDA 0 20 20] + [ABCD 5 5 21] [DABC 10 9 22] [CDAB 15 14 23] [BCDA 4 20 24] + [ABCD 9 5 25] [DABC 14 9 26] [CDAB 3 14 27] [BCDA 8 20 28] + [ABCD 13 5 29] [DABC 2 9 30] [CDAB 7 14 31] [BCDA 12 20 32] + // Round 3. + // Let [abcd k s t] denote the operation + // a = b + ((a + H(b,c,d) + X[k] + T[i]) <<< s). + // Do the following 16 operations. + [ABCD 5 4 33] [DABC 8 11 34] [CDAB 11 16 35] [BCDA 14 23 36] + [ABCD 1 4 37] [DABC 4 11 38] [CDAB 7 16 39] [BCDA 10 23 40] + [ABCD 13 4 41] [DABC 0 11 42] [CDAB 3 16 43] [BCDA 6 23 44] + [ABCD 9 4 45] [DABC 12 11 46] [CDAB 15 16 47] [BCDA 2 23 48] + // Round 4. + // Let [abcd k s t] denote the operation + // a = b + ((a + I(b,c,d) + X[k] + T[i]) <<< s). + // Do the following 16 operations. + [ABCD 0 6 49] [DABC 7 10 50] [CDAB 14 15 51] [BCDA 5 21 52] + [ABCD 12 6 53] [DABC 3 10 54] [CDAB 10 15 55] [BCDA 1 21 56] + [ABCD 8 6 57] [DABC 15 10 58] [CDAB 6 15 59] [BCDA 13 21 60] + [ABCD 4 6 61] [DABC 11 10 62] [CDAB 2 15 63] [BCDA 9 21 64] + // Then perform the following additions. (That is increment each + // of the four registers by the value it had before this block + // was started.) + A = A + AA + B = B + BB + C = C + CC + D = D + DD + end // of loop on i +3.5 Step 5. Output + The message digest produced as output is A, B, C, D. That is, we + begin with the low-order byte of A, and end with the high-order byte + of D. + This completes the description of MD5. A reference implementation in + C is given in the appendix. +4. Summary + The MD5 message-digest algorithm is simple to implement, and provides + a "fingerprint" or message digest of a message of arbitrary length. + It is conjectured that the difficulty of coming up with two messages + having the same message digest is on the order of 2^64 operations, + and that the difficulty of coming up with any message having a given + message digest is on the order of 2^128 operations. The MD5 algorithm + has been carefully scrutinized for weaknesses. It is, however, a + relatively new algorithm and further security analysis is of course + justified, as is the case with any new proposal of this sort. +5. Differences Between MD4 and MD5 + The following are the differences between MD4 and MD5: + 1. A fourth round has been added. + 2. Each step now has a unique additive constant. + 3. The function g in round 2 was changed from (XY v XZ v YZ) to + (XZ v Y not(Z)) to make g less symmetric. + 4. Each step now adds in the result of the previous step. This + promotes a faster "avalanche effect". + 5. The order in which input words are accessed in rounds 2 and + 3 is changed, to make these patterns less like each other. + 6. The shift amounts in each round have been approximately + optimized, to yield a faster "avalanche effect." The shifts in + different rounds are distinct. +References + [1] Rivest, R., "The MD4 Message Digest Algorithm", RFC 1320, MIT and + RSA Data Security, Inc., April 1992. + [2] Rivest, R., "The MD4 message digest algorithm", in A.J. Menezes + and S.A. Vanstone, editors, Advances in Cryptology - CRYPTO '90 + Proceedings, pages 303-311, Springer-Verlag, 1991. + [3] CCITT Recommendation X.509 (1988), "The Directory - + Authentication Framework." +APPENDIX A - Reference Implementation + This appendix contains the following files taken from RSAREF: A + Cryptographic Toolkit for Privacy-Enhanced Mail: + global.h -- global header file + md5.h -- header file for MD5 + md5c.c -- source code for MD5 + For more information on RSAREF, send email to . + The appendix also includes the following file: + mddriver.c -- test driver for MD2, MD4 and MD5 + The driver compiles for MD5 by default but can compile for MD2 or MD4 + if the symbol MD is defined on the C compiler command line as 2 or 4. + The implementation is portable and should work on many different + plaforms. However, it is not difficult to optimize the implementation + on particular platforms, an exercise left to the reader. For example, + on "little-endian" platforms where the lowest-addressed byte in a 32- + bit word is the least significant and there are no alignment + restrictions, the call to Decode in MD5Transform can be replaced with + a typecast. +*/ + + + diff --git a/src/entities/StatelessReader.cpp b/src/entities/StatelessReader.cpp index 18fd76ae..dd18d2db 100644 --- a/src/entities/StatelessReader.cpp +++ b/src/entities/StatelessReader.cpp @@ -109,6 +109,14 @@ bool StatelessReader::addNewMatchedWriter(const WriterProxy &newProxy) { } void StatelessReader::removeWriter(const Guid_t &guid) { + auto isElementToRemove_Instance = [&](const Instance_t &instance){ + return instance.owner->remoteWriterGuid == guid; + }; + auto thunk_instance = [](void *arg, const Instance_t &value) { + return (*static_cast(arg))(value); + }; + m_instances.remove(thunk_instance, &isElementToRemove_Instance); + auto isElementToRemove = [&](const WriterProxy &proxy) { return proxy.remoteWriterGuid == guid; }; @@ -121,6 +129,13 @@ void StatelessReader::removeWriter(const Guid_t &guid) { void StatelessReader::removeWriterOfParticipant( const GuidPrefix_t &guidPrefix) { + auto isElementToRemove_Instance = [&](const Instance_t &instance){ + return instance.owner->remoteWriterGuid.prefix == guidPrefix; + }; + auto thunk_instance = [](void *arg, const Instance_t &value) { + return (*static_cast(arg))(value); + }; + m_instances.remove(thunk_instance, &isElementToRemove_Instance); auto isElementToRemove = [&](const WriterProxy &proxy) { return proxy.remoteWriterGuid.prefix == guidPrefix; }; From 77fb5768a7f674978a2a0307bbc2d31ecc0cc5eb Mon Sep 17 00:00:00 2001 From: sven Date: Wed, 20 Oct 2021 15:16:32 +0200 Subject: [PATCH 23/25] prepare rtt --- include/rtps/entities/Domain.h | 2 +- include/rtps/entities/StatelessWriter.tpp | 1 - src/common/MD5.cpp | 223 +--------------------- src/entities/Domain.cpp | 12 +- 4 files changed, 11 insertions(+), 227 deletions(-) diff --git a/include/rtps/entities/Domain.h b/include/rtps/entities/Domain.h index 150d13ca..74d27556 100644 --- a/include/rtps/entities/Domain.h +++ b/include/rtps/entities/Domain.h @@ -51,7 +51,7 @@ class Domain { bool enforceUnicast = false); - Writer *createWriter(Participant &part, const char *topicName, const char *typeName, OwnershipKind_t ownership_kind, OwnershipStrength_t ownershipStrength, bool reliable , bool topichasKey , bool enforceUnicast); + Writer *createWriter(Participant &part, const char *topicName, const char *typeName, bool topichasKey, OwnershipKind_t ownership_kind, OwnershipStrength_t ownershipStrength, bool reliable , bool enforceUnicast); Reader *createReader(Participant &part, const char *topicName, bool topichasKey, diff --git a/include/rtps/entities/StatelessWriter.tpp b/include/rtps/entities/StatelessWriter.tpp index f601758e..ac58885e 100644 --- a/include/rtps/entities/StatelessWriter.tpp +++ b/include/rtps/entities/StatelessWriter.tpp @@ -34,7 +34,6 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/utils/udpUtils.h" using rtps::CacheChange; -using rtps::SequenceNumber_t; using rtps::StatelessWriterT; #if SLW_VERBOSE && RTPS_GLOBAL_VERBOSE diff --git a/src/common/MD5.cpp b/src/common/MD5.cpp index fc645e4c..3c2bd944 100644 --- a/src/common/MD5.cpp +++ b/src/common/MD5.cpp @@ -212,10 +212,10 @@ void rtps::MD5Init (MD5_CTX *context) context->count[0] = context->count[1] = 0; // Load magic initialization constants. - context->state[0] = 0x67452301; - context->state[1] = 0xefcdab89; - context->state[2] = 0x98badcfe; - context->state[3] = 0x10325476; + context->state[0] = 0x67452301; // A + context->state[1] = 0xefcdab89; // B + context->state[2] = 0x98badcfe; // C + context->state[3] = 0x10325476; // D } // MD5 block update operation. Continues an MD5 message-digest @@ -283,218 +283,3 @@ void rtps::MD5Final ( uint8_t digest[16], MD5_CTX *context ) } - -/* - The MD5 algorithm is designed to be quite fast on 32-bit machines. In - addition, the MD5 algorithm does not require any large substitution - tables; the algorithm can be coded quite compactly. - The MD5 algorithm is an extension of the MD4 message-digest algorithm - 1,2]. MD5 is slightly slower than MD4, but is more "conservative" in - design. MD5 was designed because it was felt that MD4 was perhaps - being adopted for use more quickly than justified by the existing - critical review; because MD4 was designed to be exceptionally fast, - it is "at the edge" in terms of risking successful cryptanalytic - attack. MD5 backs off a bit, giving up a little in speed for a much - greater likelihood of ultimate security. It incorporates some - suggestions made by various reviewers, and contains additional - optimizations. The MD5 algorithm is being placed in the public domain - for review and possible adoption as a standard. - For OSI-based applications, MD5's object identifier is - md5 OBJECT IDENTIFIER ::= - iso(1) member-body(2) US(840) rsadsi(113549) digestAlgorithm(2) 5} - In the X.509 type AlgorithmIdentifier [3], the parameters for MD5 - should have type NULL. -2. Terminology and Notation - In this document a "word" is a 32-bit quantity and a "byte" is an - eight-bit quantity. A sequence of bits can be interpreted in a - natural manner as a sequence of bytes, where each consecutive group - of eight bits is interpreted as a byte with the high-order (most - significant) bit of each byte listed first. Similarly, a sequence of - bytes can be interpreted as a sequence of 32-bit words, where each - consecutive group of four bytes is interpreted as a word with the - low-order (least significant) byte given first. - Let x_i denote "x sub i". If the subscript is an expression, we - surround it in braces, as in x_{i+1}. Similarly, we use ^ for - superscripts (exponentiation), so that x^i denotes x to the i-th - power. - Let the symbol "+" denote addition of words (i.e., modulo-2^32 - addition). Let X <<< s denote the 32-bit value obtained by circularly - shifting (rotating) X left by s bit positions. Let not(X) denote the - bit-wise complement of X, and let X v Y denote the bit-wise OR of X - and Y. Let X xor Y denote the bit-wise XOR of X and Y, and let XY - denote the bit-wise AND of X and Y. -3. MD5 Algorithm Description - We begin by supposing that we have a b-bit message as input, and that - we wish to find its message digest. Here b is an arbitrary - nonnegative integer; b may be zero, it need not be a multiple of - eight, and it may be arbitrarily large. We imagine the bits of the - message written down as follows: - m_0 m_1 ... m_{b-1} - The following five steps are performed to compute the message digest - of the message. -3.1 Step 1. Append Padding Bits - The message is "padded" (extended) so that its length (in bits) is - congruent to 448, modulo 512. That is, the message is extended so - that it is just 64 bits shy of being a multiple of 512 bits long. - Padding is always performed, even if the length of the message is - already congruent to 448, modulo 512. - Padding is performed as follows: a single "1" bit is appended to the - message, and then "0" bits are appended so that the length in bits of - the padded message becomes congruent to 448, modulo 512. In all, at - least one bit and at most 512 bits are appended. -3.2 Step 2. Append Length - A 64-bit representation of b (the length of the message before the - padding bits were added) is appended to the result of the previous - step. In the unlikely event that b is greater than 2^64, then only - the low-order 64 bits of b are used. (These bits are appended as two - 32-bit words and appended low-order word first in accordance with the - previous conventions.) - At this point the resulting message (after padding with bits and with - b) has a length that is an exact multiple of 512 bits. Equivalently, - this message has a length that is an exact multiple of 16 (32-bit) - words. Let M[0 ... N-1] denote the words of the resulting message, - where N is a multiple of 16. -3.3 Step 3. Initialize MD Buffer - A four-word buffer (A,B,C,D) is used to compute the message digest. - Here each of A, B, C, D is a 32-bit register. These registers are - initialized to the following values in hexadecimal, low-order bytes - first): - word A: 01 23 45 67 - word B: 89 ab cd ef - word C: fe dc ba 98 - word D: 76 54 32 10 -3.4 Step 4. Process Message in 16-Word Blocks - We first define four auxiliary functions that each take as input - three 32-bit words and produce as output one 32-bit word. - F(X,Y,Z) = XY v not(X) Z - G(X,Y,Z) = XZ v Y not(Z) - H(X,Y,Z) = X xor Y xor Z - I(X,Y,Z) = Y xor (X v not(Z)) - In each bit position F acts as a conditional: if X then Y else Z. - The function F could have been defined using + instead of v since XY - and not(X)Z will never have 1's in the same bit position.) It is - interesting to note that if the bits of X, Y, and Z are independent - and unbiased, the each bit of F(X,Y,Z) will be independent and - unbiased. - The functions G, H, and I are similar to the function F, in that they - act in "bitwise parallel" to produce their output from the bits of X, - Y, and Z, in such a manner that if the corresponding bits of X, Y, - and Z are independent and unbiased, then each bit of G(X,Y,Z), - H(X,Y,Z), and I(X,Y,Z) will be independent and unbiased. Note that - the function H is the bit-wise "xor" or "parity" function of its - inputs. - This step uses a 64-element table T[1 ... 64] constructed from the - sine function. Let T[i] denote the i-th element of the table, which - is equal to the integer part of 4294967296 times abs(sin(i)), where i - is in radians. The elements of the table are given in the appendix. - Do the following: - // Process each 16-word block. - For i = 0 to N/16-1 do - // Copy block i into X. - For j = 0 to 15 do - Set X[j] to M[i*16+j]. - end // of loop on j - // Save A as AA, B as BB, C as CC, and D as DD. - AA = A - BB = B - CC = C - DD = D - // Round 1. - // Let [abcd k s i] denote the operation - // a = b + ((a + F(b,c,d) + X[k] + T[i]) <<< s). - // Do the following 16 operations. - [ABCD 0 7 1] [DABC 1 12 2] [CDAB 2 17 3] [BCDA 3 22 4] - [ABCD 4 7 5] [DABC 5 12 6] [CDAB 6 17 7] [BCDA 7 22 8] - [ABCD 8 7 9] [DABC 9 12 10] [CDAB 10 17 11] [BCDA 11 22 12] - [ABCD 12 7 13] [DABC 13 12 14] [CDAB 14 17 15] [BCDA 15 22 16] - // Round 2. - // Let [abcd k s i] denote the operation - // a = b + ((a + G(b,c,d) + X[k] + T[i]) <<< s). - // Do the following 16 operations. - [ABCD 1 5 17] [DABC 6 9 18] [CDAB 11 14 19] [BCDA 0 20 20] - [ABCD 5 5 21] [DABC 10 9 22] [CDAB 15 14 23] [BCDA 4 20 24] - [ABCD 9 5 25] [DABC 14 9 26] [CDAB 3 14 27] [BCDA 8 20 28] - [ABCD 13 5 29] [DABC 2 9 30] [CDAB 7 14 31] [BCDA 12 20 32] - // Round 3. - // Let [abcd k s t] denote the operation - // a = b + ((a + H(b,c,d) + X[k] + T[i]) <<< s). - // Do the following 16 operations. - [ABCD 5 4 33] [DABC 8 11 34] [CDAB 11 16 35] [BCDA 14 23 36] - [ABCD 1 4 37] [DABC 4 11 38] [CDAB 7 16 39] [BCDA 10 23 40] - [ABCD 13 4 41] [DABC 0 11 42] [CDAB 3 16 43] [BCDA 6 23 44] - [ABCD 9 4 45] [DABC 12 11 46] [CDAB 15 16 47] [BCDA 2 23 48] - // Round 4. - // Let [abcd k s t] denote the operation - // a = b + ((a + I(b,c,d) + X[k] + T[i]) <<< s). - // Do the following 16 operations. - [ABCD 0 6 49] [DABC 7 10 50] [CDAB 14 15 51] [BCDA 5 21 52] - [ABCD 12 6 53] [DABC 3 10 54] [CDAB 10 15 55] [BCDA 1 21 56] - [ABCD 8 6 57] [DABC 15 10 58] [CDAB 6 15 59] [BCDA 13 21 60] - [ABCD 4 6 61] [DABC 11 10 62] [CDAB 2 15 63] [BCDA 9 21 64] - // Then perform the following additions. (That is increment each - // of the four registers by the value it had before this block - // was started.) - A = A + AA - B = B + BB - C = C + CC - D = D + DD - end // of loop on i -3.5 Step 5. Output - The message digest produced as output is A, B, C, D. That is, we - begin with the low-order byte of A, and end with the high-order byte - of D. - This completes the description of MD5. A reference implementation in - C is given in the appendix. -4. Summary - The MD5 message-digest algorithm is simple to implement, and provides - a "fingerprint" or message digest of a message of arbitrary length. - It is conjectured that the difficulty of coming up with two messages - having the same message digest is on the order of 2^64 operations, - and that the difficulty of coming up with any message having a given - message digest is on the order of 2^128 operations. The MD5 algorithm - has been carefully scrutinized for weaknesses. It is, however, a - relatively new algorithm and further security analysis is of course - justified, as is the case with any new proposal of this sort. -5. Differences Between MD4 and MD5 - The following are the differences between MD4 and MD5: - 1. A fourth round has been added. - 2. Each step now has a unique additive constant. - 3. The function g in round 2 was changed from (XY v XZ v YZ) to - (XZ v Y not(Z)) to make g less symmetric. - 4. Each step now adds in the result of the previous step. This - promotes a faster "avalanche effect". - 5. The order in which input words are accessed in rounds 2 and - 3 is changed, to make these patterns less like each other. - 6. The shift amounts in each round have been approximately - optimized, to yield a faster "avalanche effect." The shifts in - different rounds are distinct. -References - [1] Rivest, R., "The MD4 Message Digest Algorithm", RFC 1320, MIT and - RSA Data Security, Inc., April 1992. - [2] Rivest, R., "The MD4 message digest algorithm", in A.J. Menezes - and S.A. Vanstone, editors, Advances in Cryptology - CRYPTO '90 - Proceedings, pages 303-311, Springer-Verlag, 1991. - [3] CCITT Recommendation X.509 (1988), "The Directory - - Authentication Framework." -APPENDIX A - Reference Implementation - This appendix contains the following files taken from RSAREF: A - Cryptographic Toolkit for Privacy-Enhanced Mail: - global.h -- global header file - md5.h -- header file for MD5 - md5c.c -- source code for MD5 - For more information on RSAREF, send email to . - The appendix also includes the following file: - mddriver.c -- test driver for MD2, MD4 and MD5 - The driver compiles for MD5 by default but can compile for MD2 or MD4 - if the symbol MD is defined on the C compiler command line as 2 or 4. - The implementation is portable and should work on many different - plaforms. However, it is not difficult to optimize the implementation - on particular platforms, an exercise left to the reader. For example, - on "little-endian" platforms where the lowest-addressed byte in a 32- - bit word is the least significant and there are no alignment - restrictions, the call to Decode in MD5Transform can be replaced with - a typecast. -*/ - - - diff --git a/src/entities/Domain.cpp b/src/entities/Domain.cpp index eccdc80d..e74bdcae 100644 --- a/src/entities/Domain.cpp +++ b/src/entities/Domain.cpp @@ -307,11 +307,11 @@ rtps::Writer *Domain::writerExists(Participant &part, const char *topicName, } -rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, const char *typeName, - OwnershipKind_t ownership_kind, OwnershipStrength_t ownershipStrength, - bool reliable , - bool topichasKey, - bool enforceUnicast ){ +rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, + const char *typeName,bool topichasKey, + OwnershipKind_t ownership_kind, OwnershipStrength_t ownershipStrength, + bool reliable , + bool enforceUnicast ){ // Check if there is enough capacity for more writers if ((reliable && m_statefulWriters.size() <= m_numStatefulWriters) || ((!reliable) && m_statelessWriters.size() <= m_numStatelessWriters) || @@ -368,7 +368,7 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, con rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, const char *typeName, bool reliable, bool enforceUnicast) { - return createWriter(part,topicName,typeName,OwnershipKind_t::SHARED, 0, reliable, false, enforceUnicast); + return createWriter(part,topicName,typeName,false,OwnershipKind_t::SHARED, 0, reliable, enforceUnicast); } rtps::Reader *Domain::createReader(Participant &part, const char *topicName, bool topichasKey, From 91c1df900db3fe16889c7c1188d80ec0120ec8f5 Mon Sep 17 00:00:00 2001 From: sven Date: Sat, 23 Oct 2021 18:09:16 +0200 Subject: [PATCH 24/25] fixed an ownership bug, rtps example dedicated to ownership test --- include/rtps/common/types.h | 5 +---- src/entities/StatelessReader.cpp | 4 ++-- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/include/rtps/common/types.h b/include/rtps/common/types.h index 27bd2f3a..689a751e 100644 --- a/include/rtps/common/types.h +++ b/include/rtps/common/types.h @@ -234,12 +234,9 @@ enum class ChangeForReaderStatusKind { enum class ChangeFromWriterStatusKind { LOST, MISSING, RECEIVED, UNKNOWN }; -struct InstanceHandle_t { // TODO +struct InstanceHandle_t { uint8_t key[16]; - //uint64_t key; bool operator==(const InstanceHandle_t &other) const { - return key == other.key; - for (int i = 0; i < 16; i++) { if(key[i] != other.key[i]) { return false; diff --git a/src/entities/StatelessReader.cpp b/src/entities/StatelessReader.cpp index dd18d2db..08e74a8d 100644 --- a/src/entities/StatelessReader.cpp +++ b/src/entities/StatelessReader.cpp @@ -46,6 +46,7 @@ void StatelessReader::init(const TopicData &attributes) { bool StatelessReader::isOwner(InstanceHandle_t &handle, WriterProxy *proxy){ for(auto &instance : m_instances){ + bool keyEqual = instance.handle == handle; if(instance.handle == handle){ if(instance.owner == nullptr){ instance.owner = proxy; @@ -67,7 +68,7 @@ bool StatelessReader::isOwner(InstanceHandle_t &handle, WriterProxy *proxy){ } } } - } + }//Instance not knwon Instance_t instance; instance.owner = proxy; instance.handle = handle; @@ -89,7 +90,6 @@ void StatelessReader::newChange(const ReaderCacheChange &cacheChange) { else{ m_callback(m_callee, cacheChange); } - m_callback(m_callee, cacheChange); } } From 9c21365d8e3a7eb6653770f24951d097c68f891b Mon Sep 17 00:00:00 2001 From: sven Date: Tue, 26 Oct 2021 01:12:20 +0200 Subject: [PATCH 25/25] chibios --- include/rtps/common/types.h | 2 -- include/rtps/entities/StatefulReader.tpp | 1 + include/rtps/messages/MessageFactory.h | 2 +- src/entities/Domain.cpp | 12 +++++++++--- 4 files changed, 11 insertions(+), 6 deletions(-) diff --git a/include/rtps/common/types.h b/include/rtps/common/types.h index 4029967f..f1448073 100644 --- a/include/rtps/common/types.h +++ b/include/rtps/common/types.h @@ -283,8 +283,6 @@ const SequenceNumber_t SEQUENCENUMBER_UNKNOWN = {-1, 0}; const Time_t TIME_ZERO = {}; const Time_t TIME_INVALID = {-1, 0xFFFFFFFF}; -const Time_t TIME_INFINITY = {0x7FFFFFFF, 0xFFFFFFFF}; - const Time_t TIME_INFINITY = {0x7FFFFFFF, 0xFFFFFFFF}; class WriterProxy; diff --git a/include/rtps/entities/StatefulReader.tpp b/include/rtps/entities/StatefulReader.tpp index c557e574..1aea4dc4 100644 --- a/include/rtps/entities/StatefulReader.tpp +++ b/include/rtps/entities/StatefulReader.tpp @@ -114,6 +114,7 @@ void StatefulReaderT::newChange( if (proxy.expectedSN == cacheChange.sn) { ++proxy.expectedSN; if(m_attributes.ownership_Kind == OwnershipKind_t::EXCLUSIVE) { + InstanceHandle_t handle; m_KeyCallback(cacheChange.getData(), cacheChange.getDataSize(), handle); if (isOwner(handle, &proxy)) { // m_callback(m_callee, cacheChange); diff --git a/include/rtps/messages/MessageFactory.h b/include/rtps/messages/MessageFactory.h index c9c2b683..e16cdd8f 100644 --- a/include/rtps/messages/MessageFactory.h +++ b/include/rtps/messages/MessageFactory.h @@ -101,7 +101,7 @@ void addSubMessageTimeStamp(Buffer &buffer, bool setInvalid = false) { template void addSubMessageData(Buffer &buffer, const Buffer &filledPayload, bool containsInlineQos, const SequenceNumber_t &SN, - const EntityId_t &writerID, const EntityId_t &readerID) { + const EntityId_t &writerID, const EntityId_t &readerID, const OwnershipKind_t &ownershipKind = OwnershipKind_t::SHARED, const OwnershipStrength_t &ownershipStrength = 0) { SubmessageData msg; msg.header.submessageId = SubmessageKind::DATA; #if IS_LITTLE_ENDIAN diff --git a/src/entities/Domain.cpp b/src/entities/Domain.cpp index d4d490fb..0a0e3ee4 100644 --- a/src/entities/Domain.cpp +++ b/src/entities/Domain.cpp @@ -416,9 +416,9 @@ rtps::Reader *Domain::createReader(Participant &part, const char *topicName, boo } } attributes.durabilityKind = DurabilityKind_t::VOLATILE; - TopicKind_t kind; + //TopicKind_t kind; if(topichasKey){ - kind = TopicKind_t::WITH_KEY; + // kind = TopicKind_t::WITH_KEY; attributes.endpointGuid.entityId.entityKind = EntityKind_t::USER_DEFINED_READER_WITH_KEY; } DOMAIN_LOG("Creating reader[%s, %s]\n", topicName, typeName); @@ -468,9 +468,15 @@ rtps::GuidPrefix_t Domain::generateGuidPrefix(ParticipantId_t id) const { GuidPrefix_t prefix = Config::BASE_GUID_PREFIX; #if defined(unix) || defined(__unix__) srand(time(nullptr)); -#else +#elseif defined(CHIBIOS) + unsigned int seed = (int)chVTGetSystemTimeX(); + srand(seed); +#elseif defined(CHIBIOS) unsigned int seed = (int)xTaskGetTickCount(); srand(seed); +#else + unsigned int seed = (int)chVTGetSystemTimeX(); + srand(seed); #endif for (auto i = 0; i < rtps::Config::BASE_GUID_PREFIX.id.size(); i++) { prefix.id[i] = (rand() % 256);